Mathematics  Research  Reports 


D D C 

FEB  71  1879 


Division  of  Mathematics  and  Physics 

UNIVIRSITY  OP  MARYLAND 
BALTIMORI  COUNTY 


Baltimore,  Maryland  21228 

AIR  loses  omes  or  scinmnc  women  (AFM) 

nones  or  trmbsxitul  to  doc 

Thla  technical  report  has  b*M  ml»r#4  Mi  if 

approved  fer  r&lie  nlOM  lil  ill  SMf  ifjM#  jDn„ 

DistrltaUn  is  maUmUM^,  df:d  0v®d  for 

*U  ft.  KbQSK  ®*plbutlon 


dlatpibuti»  PUb2l0  rel.. 

Utlon  toHait9d  # s*'' 


janaiwn* ~ 

i— • m 


^ VL*  '■'? 

r >* 

I 0 

i 

| !±i 


] I 

1 - ■•"  ~ Ea 

7/ .. 


Isl 


SI 

(Z) 


Optimal  Estimation  for  the  Satellite  Attitude 
/using  Star  Tracker  Measurements / 


nes  Ting 


-Ho/L 


Mathematic^  Research  Report  No" 
(fi]* ov—W,  W78] 

^ 7/4 


r / 

nTotL78- 


4^K~/ 


ABSTRACT 


D D C 

2©W."SUS2:j? 

FEB  2 1 1979  ? 

i 

ETEJranrtitli 


An  optimal  estimation  scheme  ..s  presented,  which  determines  the  satellite 
attitude  using  the  gyro  readings  and  the  star  tracker  measurements  of  a commonly 
used  satellite  attitude  measuring  unit.  The  scheme  is  mainly  based  on  the 
exponential  Fourier  densities  that  have  the  desirable  closure  property  under 
conditioning.  By  updating  a finite  and  fixed  number  of  parameters,  the  condi- 
tional ni-cbability  density,  which  is  a exponential  Fourier  density,  is  recur- 
sively determined. 

Simulation  results  indicate  that  the  scheme  is  effective  and  robust.  It 
is  believed  that  this  approach  is  applicable  to  many  other  attitude  measuring 
units.  As  t.-'  linearization  and  approximation  are  necessary  in  the  approach, 
it  is  ideal  for  systems  involving  high  levels  of  randomness. 

When  a system  involves  little  randomness  and  linearization  is  not  expected 
to  incur  much  error,  the  approach  can  provide  a benchmark  against  which  such 
suboptimal  estimators  as  the  extended  Kalman  filter  and  the  least-squares  esti- 
mator can  be  compared.  In  this  spirit,  simulated  data  for  HEAO-A  were  processed 
to  compare  the  optimal  scheme  and  the  extended  Kalman  filter.  The  results  are 
presented. 


This  work  was  supported  by  the  Goddard  Space  Flight  Center/NASA  under 
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I.  Introduction 

The  project  being  reported  on  is  mainly  concerned  with  estimating 
the  satellite  attitude  given  the  gyro  readings  and  the  star  tracker 
measurements  of  a commonly  used  satellite  attitude  measuring  unit  (SAMU) . 
The  SAMU  is  used  in  such  satellites  as  the  high  energy  astronomy  observa- 
tory (11HA0)  and  the  precision  pointing  control  system  (PPCS)  [1]  [2]. 

It  is  composed  of  3 to  6 rate  gyros  and  2 star  trackers.  The  satellite 
attitude  is  propagated  over  a certain  number  of  small  time  intervals  by 
integrating  the  satellite  angular  rates  determined  from  the  gyro  reading. 
Gyro  drift  rates,  misalignments,  and  lack  of  a precise  initial  attitude 
reference  then  make  it  necessary  to  employ  two  gimbaled  star  trackers 
to  provide  a bench  mark  to  the  further  propagation  of  the  satellite 
attitude.  A star  tracker  utilizes  an  image  dissector  tube  to  locate 
the  position  of  a star  on  its  photosensitive  surface.  Due  to  the  non- 
stationary nonlinear  characteristics  of  the  image  dissector  deflection 
coils  and  the  white  noise  from  the  processing  electronics,  it  is  at  this 
stage  that  estimation  is  required. 

A new  representation  of  a probability  density  of  a three  dimensional 
rotation  called  the  exponential  Fourier  density  (EFD)  , was  recently  intro- 
duced [3]  14],  which  has  the  desirable  closure  property  under  the  operation 
of  taking  conditional  distributions.  Using  the  EFD's,  an  approach  was  sug- 
gested in  [3]  [4]  to  derive  recursive  formulas  for  updating  the  conditional 
densities  of  a rotational  process  given  a nonlinear  observation  in  additive 
white  noise. 

In  this  report,  this  approach  is  carried  out  for  the  aforementioned 
satellite  attitude  estimation  problem.  The  recursive  formulas  for  updating 
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the  conditional  densities  of  the  satellite  attitude  are  derived  for  arbi- 
trary star  tracker  equations.  These  general  formulas  are  included  here  to 
accommodate  possible  future  consideration  of  the  distortion  characteristics 
of  the  image  dissector  deflection  coils  [1]  [2]  and  possible  future  change 
in  the  star  tracker  configuration.  These  general  formulas  also  provide  a 
basis  on  which  special  cases  can  be  easily  analyzed.  However,  they  involve 
a large  amount  of  computation.  Their  feasibility  for  on-board  implementa- 
tion is  highly  questionable. 

In  a conversation  with  ii.  J.  Lefferts  of  the  GSFC/NASA,  it  was 
observed  by  him  that  by  choosing  appropriately  the  mathematical  description 
of  the  star  tracker  configuration,  the  star  tracker  measurement  can  be  ex- 
pressed in  closed  form  as  a linear  conbination  of  the  rotational  harmonic 
functions  of  order  one.  This  observation  substantially  simplifies  the 
optimal  estimation  scheme  and  greatly  reduces  the  amount  of  computation 
required  in  both  designing  and  utilizing  the  scheme.  A detailed  deriva- 
tion of  the  associated  equations  is  included  in  the  sequel.  It  is  these 
equations  that  we  use  in  implementation,  simulation,  and  evaluation. 

A total  of  37  computer  programs  were  created  to  carry 
out  essentially  the  following  tasks: 

TASK  1.  Simulating  the  satellite  attitude  propagation  and  measurement; 

TASK  2.  Updating  the  conditional  density  using  the  recursive  formulas 

for  its  Fourier  coefficients; 

TASK  3.  Integrating  the  conditional  covariance  matrix  of  the  attitude 
quaternion; 

TASK  4.  Computing  the  maximum  eigen-value  and  its  eigenvector  of  the 
conditional  covariance  matrix  and  the  estimation  errors; 
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TASK  5.  Searching  for  the  maximum  likehood  estimate. 

TASK  6.  Plotting  the  estimation  errors. 

A close  look  at  the  mathematical  models  (30)  and  (31)  for  the  star  tracker 
measurement,  which  are  either  used  in  [1]  and  [2]  or  modified  by  E.  J.  Lefferts 
of  the  (>SFC/NASA,  revealed  that  the  measurement  models  are  not  observable. 

The  nonobservabi 1 ity  causes  a pseudo-image  of  each  observed  star.  As  the  ex- 
tended K-B  filtering  is  merely  a local  processing,  it  does  not  pick  up  the 
pseudo-image  and  is  therefore  immune  from  its  effect.  In  contrast,  the  optimal 
scheme  does  not  have  any  "blind  spots"  and  thus  assigns  an  equal  probability 
to  the  double  images  of  each  of  the  observed  stars.  An  example  illust- 
rating such  nonobservability  is  given  in  Section  VI. 

Fortunately  enough,  this  difficulty  resulting  from  the  nonobserv- 
ability can  be  remedied  by  introducing  a "pseudo-measurement''  of  the 
second  apparent  star  direction  cosine  u2(k)  with  respect  to  the  tracker 
base  reference  axes.  We  note  that  U2(k)  is  the  component  of  the  direc- 
tion v tor  u(k)  that  is  perpendicular  to  the  tracker  field-of-view  and 
hence  can  not  be  measured  directly  by  the  tracker.  However, from  using 
the  satellite  attitude  estimate  s(t)  at  the  previous  step  t=k-l,  u2(k) 
can  be  predicted,  which  is  to  be  used  as  a "measurement"  of  the  real  u2(k). 
Facilitated  with  such  a pseudo-measurement,  the  pseudo-image  of  the  ob- 
served star  can  be  eliminated.  For  want  of  a mathematically  rigorous 
proof,  only  a heuristic  explanation  for  this  pseudo-measurement  approach, 
which  is  believed  to  be  new,  is  given  in  Section  VII. 

Among  the  six  tasks  mentioned  above,  TASK  3 is  most  CPU-time-consuming, 


which  involves  nine  integrations  on  the  3-dimensional  rotation  group. 
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Hncouraging  is  the  fast  and  high  concentration  of  the  conditional  probability 
density  at  the  satellite  attitude  under  estimation.  This  phenomenon  is 
dictated  by  the  theory  and  suggest  two  possible  ways  to  get  around  the  diffi- 
culty of  integration.  One  way  is  to  localize  the  integration.  Another  way 
is  to  use  the  maximum  likelihood  estimate  instead  of  the  optimal  estimate, 
both  methods  were  researched  and  implemented  on  the  computer.  The  simulation 
results  indicate  that  there  is  virtually  no  difference  between  the  estimates 
obtained  in  these  two  ways  (at  least  for  the  models  used  in  the  project). 

The  maximum  likelihood  estimator  avoids  not  only  integration  altogether 
but  also  TASK  4-computing  the  maximum  eigenvalue  and  its  eigenvector.  All  it 
needs  is  the  updated  Fourier  coefficients  of  the  conditional  density,  which 
are  obtained  through  simple  algebraic  formulas.  Therefore,  the  maximum  like- 
lihood estimator  is  used  in  comparison  with  the  extended  K-B  filter,  a stan- 
dard method  for  spacecraft  attitude  estimation. 

A comparison  between  the  K-B  filtering  and  our  maximum  likelihood  esti- 
mator .s  conducted  by  E.  J.  Lefferts  of  the  GSFC/NASA.  A.  N.  Mansfield  of 
the  CS'I’A  generated  a sequence  of  33  star  tracker  observations.  The  average 
body  rates  were  provided  every  one  third  of  a second,  and  the  tracker  ob- 
servation was  taken  every  two  minutes.  The  standard  deviation  of  the  tracker 
measurement  noises  is  20  arcseconds.  For  such  a low  noise  level,  it  is 
known  that  linearization  is  a very  good  approximation.  Therefore,  it  is 
not  surprising  that  the  maximum  likelihood  estimator  is  not  much  better  than 
the  K-B  filter. 

However,  the  comparison  results  indicate  that  the  maximum  likelihood 
estimator  is  almost  always  better  and  converges  faster  than  the  K-B  filter. 

It  is  also  noted  that:  (1)  The  simulated  measurement  data  are  in  strict 
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accord  with  the  system  model,  assuming  all  the  true  values  of  the  model 
parameters  and  the  noise  statistics  are  given.  (2)  There  is  no  random 
driving  term  in  the  state  dynamics  model  for  the  spacecraft  attitude.  Under 
these  two  conditions  in  addition  to  the  low  measurement  noise  level  men- 
tioned above,  even  simple-minded  estimators  can  be  expected  to  perform  near 
optimal.  But  these  conditions  are  usually  far  from  being  met  in  reality. 

The  simulated  examples  in  Section  IX,  used  to  compare  the  ML  and  the 
optimal  estimates,  werechosen  to  test  the  robustness  of  our  new  schemes  and 
represent  tougher  working  conditions  than  the  real  ones.  The  simulation 
results  to  be  depicted  in  graphs  indicate  that  both  the  local  integration 
estimator  and  the  maximum  likelihood  estimator  track  the  signal  nicely. 
Unfortunately,  we  did  not  get  to  use  the  K-B  filter  for  these  cases.  But, 
we  believe  that  the  extended  K-B  filter  is  no  more  valid  especially  for  the 
third  example. 

The  robustness  of  an  estimator  toward  uncertainty  of  system  parameters 
and  the  random  driving  term  in  the  state  dynamics  is  perhaps  the  most  impor- 
tant consideration  in  real  world  application.  While  there  is  every  reason 
to  believe  that  the  maximum  likelihood  estimator  is  superior  in  this  regard, 
the  issue  remains  to  be  resolved  in  hopefully  our  next  project. 

A description  and  a flow  chart  of  each  of  the  computer  programs  are 
given  in  the  Appendix.  A listing  can  be  obtained  from  the  data  set  call- 
ed W9MXN . TF.STAL . FORT , which  is  stored  in  the  IBM360/95  at  the  GSFC/NASA. 

The  programs  involved  in  each  of  the  aforementioned  tasks  are  listed  as 


follows: 
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TASK  1 . 
TASK  2. 

1 ASK  3. 
TASK  4. 
TASK  5. 
TASK  6. 


M0SSO3  (Main  Program)  , TRANS,  NEWO,  QTN,  QXQ,  RANIH1 
ADC,  COEFF1 , DR 1 2 , FU,  GAUSS,  GAUSS4 , NPAQ,  1N1TPQ,  FR,  SD, 
C3J,  DR  II 

ERR,  INTGLE,  SI  NCOS,  INSDD,  INTGL2,  DMN0,  BLK0,  ESTIM 
ERR,  EIGEN,  EIGVAL,  EIGVEC,  QVQ,  TRIDMX 
MAXI,  SEARCH,  QTA 
M06PLT,  PLOT1,  UPDIVN 


II.  Propagation  of  the  Satellite  Attitude 

The  SAMU  uses  an  inertial  measuring  unit  (IMU)  composed  of  3 to  6 
rate  gyros,  and  two  star  trackers.  Let  the  time  t be  indexed  by  a dis- 
crete intergration  step  T.  A reading  of  the  IMU  is  then  taken,  and  com- 
pensations are  applied  by  the  computer  to  yield  the  estimated  rate  act- 
ing upon  the  satellite.  This  compensation  is  based  upon  the  estimates 
of  the  gyro  misalignments  and  drift  rates.  The  compensated  output,  the 
three  estimated  angular  velocities  [wi,U2,W3]  of  the  satellite,  is  then 
integrated  to  yield  the  estimated  satellite  attitude  quaternion  p = [£,n,C,x] 
as  follows  [1]  [2] : 
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As  the  1MU  parameters  can  be  calibrated  very  accurately  before  the 
satellite  is  launched  and  the  gyro  noise  level  is  extremely  low,  the  estima- 
tion error  of  (wi  ,u>2  .<*>3)  is  assumed  negligible  to  simplify  the  project.  Hence 
if  the  initial  attitude  were  known  and  if  the  IMU  and  computer  performed  ideal- 
ly, then  the  attitude  could  be  predicted  precisely  for  all  future  time  and  no 

additional  attitude  measuring  device  would  be  required.  Unfortunately  this 
is  not  the  case  and  two  star  trackers  are  utilized  once  every  N time  steps 

to  acquire  additional  information  about  the  satellite  attitude. 


As  we  are  only  concerned  in  this  project  with  filtering  t.ne  star 
tracker  data  to  estimate  the  satellite  attitude,  the  attitude  propagation 
between  two  consecutive  star  tracker  measurements  can  be  c.'Wi.ji  . to  yield 
the  following  state  equation: 


^k+1  = 2k5* 


(2) 


where  P*.  = PRN  and 
We  note  that  each  time 


N-l 

% = j=0  Q*N+j 
step  in  (2)  is  NT. 


Let  p^  and  be  identified  with  their  Euler  angle  representations 
Sk^k’6k'^k  ^ and  wk(cV8k'V  resPectively . The  equation  (2)  can  thus  be 
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rewritten  in  terms  of  Euler  angles  as 


Vl^k+l'Vl’Vl5  “ wk(Vek'Yk)oSkCVek'V  (3) 

where  • denotes  the  product  rotation  and  (a.g.y)  is  related  to  the  components 
of  9.  through 

1_ 

* * m^h  + Cffl>21>  (®41  + <S>W  (4) 

sin  a - - C(2)nC2)31  * (S)21®41^X 
cos  a - mn(S>2i  - ^ii^41)A 

cos  0 » 2 C (9.)^  ♦ i$)221)  -l,  0 < p<  tt 

sin  y - ((9)21(9)4i  - 

cos  y * - ua)n(a)41  * (©31(ffl21)A 

Hence  the  time  sequence  [ak>ek,Ykj  can  be  easily  determined  from  the  sat- 
ellite angular  velocity  [aij  through  (1)  - (4).  This  sequence  will  be 

used  in  the  optimal  estimation  equations  to  be  derived  in  the  sequel. 

III.  General  Recursive  Formulas  for  Conditional  Densities 

The  star  tracker  measurements  (to  be  denoted  by  m^)  are  nonlinear 
functions  of  the  satellite  attitude  s^  corrupted  in  additive  white  noise 
(to  be  denoted  by  v^) . A general  mathematical  model  can  be  written  as 


mk  ■ h(sk,k)  ♦ vk  (5) 

where  n^,  h(skk),  and  vk  are  all  r-vectors.  The  measurement  noise  vk  is 
caused  by  the  processing  electronics  of  the  star  trackers  and  it  is  customari- 
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Is  assumed  Gaussian.  Wc  write  it:;  density  function  as 


P(vv)  = 


r 1^ 

(2n)  “(detR^)  ^exp 


4 E 

- ifr=i  k k k 


(6) 


where  R^  is  the  ixj-th  component  of  the  covariance  matrix  R of  v,  = [v* , ...  vr]^ 
K K k k k 

As  suggested  in  [3]  and  [4],  the  function  h(-,  k)  for  each  k must  be  appro- 
ximated very  closely  by  a finite  Fourier  series.  By  a slight  abuse  of  the  symbol 
h,  it  will  also  be  used  to  denote  its  Fourier  approximate,  i.e. 
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mn 

angles  (<f>,6,t l>)  , bY 


l 

Umn^’0’^^  = iT1'nexPl'i^m<i,+n'J;^dmn^^  » i = t'1)2 


(8) 


mn 


(0)  = 


l 

y r nt  [ (l+m) ! (l-m)  1 (l+n)  ! f-E-n) ! ] 2 
— 1 J (£+m-t) ! (t+n-m) ! t ! Ct-n-t) ! 


2£+m-n-2t 

• cos 


6^ 

2 


sin 


2t+n-m 


9_ 

2 


(9) 


i 


As  h is  a real  function,  the  above  Fourier  series  can  be  written  in  the 
alternative  form,  called  the  real  form  as  contrasted  to  the  complex  form 
(7), 


h(sk,k)  - 2^ioo^k^D00^Sk^  + 


t J 

m,n=-E  L 
m<n 

or  m=n>0 


hf  (k)ReD^  (s.)  + h^  (k)ImD>C  (s.) 
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where  the  real  coefficients  h,  and  h.  are  related  with  the  complex  coef- 
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ficient  h^  through 
inn 


f l l l 

li  (h,  - ih^  ) 

mn  2 lmn  - 2mn 


(ID 


l*  i P ] 

The  i-th  components  of  h and  Ir  will  be  denoted  bv  h and  h respectively. 

1 mn  mn  ' J 


A set  of 


Fortran  IV  programs  to  compute  *s  and  the  approximation 


mn 


error  was  written  and  successfully  tested  on  the  I BM360/95  at  the  (iSF'C/NASA. 

Descriptions  and  flow  charts  of  the  programs  are  given  in  the  Appendix. 

k - 1 

It  is  assumed  that  the  conditional  density  of  ^ given  m : = {m  , ...,mk  j) 
is  an  exponential  Fourier  density  of  order  2L,  i.e. 
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mn  k - 1 


(12) 


for  k equal  to  a positive  integer.  We  note  that  a rotational  normal  density  is 
an  exponential  Fourier  density  of  order  1.  The  reason  for  choosing  the  order 

2L  will  become  clear  in  the  following  derivation  of  the  recursive  formulas 

c £k 
for  n 
■ mn 

By  Bayes*  rule,  we  have 


k k - 1 

p(sk!m  ) = ckp(mk|sk)p(sk|m  ' ) 


ck  = a normalizing  constant 


(13) 


As  wk  is  a group  element,  from  (3)  it  follows  that  sk_j 
Substituting  this  into  (12)  yields 
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where  E = E E and  the  second  equality  holds  because  I)m 
£=0  m,m=~£ 

representation  of  the  three  dimensional  rotation  group. 
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is  a matrix 


From  (5),  (6),  and  the  definition  of  a conditional  density,  it  is  clear 


that 
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1 y R^(m*h^(k)  + mjh£l(k)) 

2 l—  k k mn v 1 k mn'  " 

i.  j = l 


i»J  = l 


(16) 


C 


l 
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C:  (m,n,m',n') 


4)  Rj*h£l'k)h£|,J’1(k) 

2 Z_ i k mn  J m 1 n ' 

i ,j  = l 


(17) 


Furthermore,  it  is  known  [5,  p.160]  that 


t+v 


p V 

U"  (s.)D  , , (s.) 

mn  k m ' n 1 k 


- £ 

q=q 


(2q+l)  (-1 ) 


-n "/  It  q 
m m'  -m' 


/ 


0 


q 0q  (s  ) 
n n'  -n"  m"n"  ^ 


(18) 


in"  = ni  + m' 
n"  = n + n' 

qQ  = max  ( | l-V  \ , |m"| , |n"| ) 


j k l 
m n p 


(-D 


2j -k+n 


(j+k -l ) ! (k+i-j)  ! (Z+j  -k)  ! (l+p)  ! (L-p)  ! 
(j+k+£+l) ! (j+m) ! (j-m) ! (k+n) ! (k-n) ! 


£ (.j)* (Z*j  -n-t)  ! (k+n  »t) ! 

t (£+p-t) ! (t+k-j-p) !t ! (t-k+j-t) ! 


(19) 


where  the  sum  on  t is  ever  values  such  that  the  arguments  of  all  the 
factoiial  functions  are  nonnegative,  Let  us  define  the  symbols: 


Z m n p p 1 rji  n»i 

Y I : = C<X(m>n,m\n')(2q+n(-ir 

V m'  n' 


/ 


l l'  q \ L l’  q 

m m -m"  n n'  -n"l 


(20) 


Tq  (t,r,  m",n")  : = 


-Z£m,n  <JL 
m"-£k  m <m"+£' 
n"-£k  n <n"+£' 


Z m n 
V m"  n"-n 


(21) 
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l 

I 

I 


I 


r"  : - Z 

mn  l,f=0 


rq|£,£’ ,m,n) 


(22) 


1 1 -l’\<q<W 


Jsing  essentially  (18),  the  last  term  in  (IS)  is  transformed  into  a linear 
combination  of  the  rotational  harmonics  as  follows: 

l l' 

m'  n ' n f c re  i 


l 

t 

V 

1 E 

E 

E 

rw , 

C (i 

II 

* 

0 m,  n =-t  ni 

1 ,n' 

} 

L 

t 

V 

l*V 

l ^ 

• l,V  = 

z 

E 

z 

0 m,n=-£  m1 

' ,n'=~l' 

q=qo 

s 

1 2L 

q 

r L 

; E 

1 

E 

E 

q=0 

m" ,n"=-q 

[ 

• II 

o 

l m n 
V m'  n' 


Dq„  „(s.) 
m"n"  k 


rq(^,£',m",n") 


I i-v  |<  q <z+r 


Dq  „(s.) 

m"n"v  k' 


21  q 

E E (»v). 

*-•  n.n  mn  k 
q=0  m,n=-q 

Substituting  (22),  (15),  and  (14)  into  (13),  we  obtain 


| 

I 


| P(sk|mk)  = exp 

I 


r 2L  l 

z z 

1=0  m,n=-t 


L.  p ’ D (w.  n) 
» rqn  qn  k-1 


q=_i  "qn  qn 
\ 


I 

I 

I 


+ rmn  + x[0,L](£)  CL 


0l  (s.  ) 
mnv  k' 


(23) 


(24) 


the  constants  ck,  c'k,  and  CQ  being  absorbed  into  the  coefficient  of  Dg0(sk)  * 1, 
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which  is  a normalizing  constant,  and  x^0  Lj  is  the  characteristic  function  for 
fO.Lj. 

We  note  that  p(SjJm  ) is  also  an  exponential  Fourier  density  of  order 

2L.  Comparing  (12)  and  (24),  we  obtain  the  recursive  formula  for  p^k- 

mn 

£ 

Pmn  " Pqn  Dqm^wk-p  + ^ + X[0,L]^Cmn 

where  F and  C are  defined  by  (20) -(22)  and  (16). 

The  function  enclosed  in  the  brackets  in  (24)  is  a real  function.  It  is 
often  easier  to  express  it  in  the  real  form  of  the  Fourier  series.  Using  the 
notational  convention  established  in  (10)  and  (11),  the  equation  (24)  can  be 
written  as 


p(s.  |mk)  = 


exp  ) 
£=[ 3 


1 ^ 

2 p100D00(sk*  + 


l 

£ 


m,n=-£ 

m<n 

or  m=n>0 


£k  t 
p,  ReD  (s.) 
lmn  mn1  k' 


+ pfk  ImD^  (s,  )| 
r2mn  mnv  ky| 


(26) 


1 Ok 


2^100  = t^ie  normal izing  constant 


The  recursive  formulas  for  p^  and  p^  can  be  deduced  from  (75)  as  follows 


l l 

V fw*1  _ y 1 / ■ l,k-l 

pqn  qn  wk-l^  “ *-• -2  I Plqn  * i?2qn 
q = -£  ' 


ReDt»(Vi>  * 


3 

I 
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£ \ |pf'k_1ReD*  (w*1  ) + pf'k_1ImD^  (w"1  ) 

^ l 2 |llctn  qm  *2qn  qm  k-1' 

I £,k-lx  , -1  . £,k-ln  U , -1 
+ i p,  ImD  (w.  ) - p ' ReD  (w.  ,) 

— \ 1 lqn  qm  k-1  r2qn  qm  k-1' 


cL4  £ 

1,3  = 1 

if  = £ Aq.q'.m.n)  « £ £ 

q,q'=o  q,q'=o  -q<t,s<q 

|q-q’|<*<q+q‘  | q - q ' | <^€<^q+q*  m-q'<t<m+q' 

~ n-q’<s<n+q' 


q t S 
q'  m-t  n-s 


q'  m-t  n-s 


p q t s \ /qq'  £\/qq'  £\  , 

t I = (-1)  (2£+l)  ] I cqq  (t,s,m-t,n-s) 


t m-t  -m  / \ s n-s  -n 


Cqq  (t,s,m-t ,n-s)  = - 


k S 

i,  3=1 


hqi  hq'j 


hq;  hq,i 


Its  1, m-t, n-s  2ts  2, m-t, n-s 


_ , hqi  hq’j  . hqi  hq'j 

—1  2tsnl ,m-t ,n-s  nltsr 2 ,m-t ,n-s 


£k  IV  IV  _ OT  IV 

^lmn  " e^mn  * ^2mn  m^mn 


pfk  = £ pf’k_1ReD*  (w"1,)  + p5,k-1ImD^  (w'1.)  ) 

plmn  _ plqn  qm1  k-1'  ^2qn  qm1  k -V  J 


*x[0>L]«).?  tf-Mi,*  \ „ £ 

1 J 1,3=1  q.q '=0  -q<t,s<q 

jq-q'litSq+q'  m-q'st<q'+m 
n=q'<,s<n+q' 


(_l)m"n+l  2<f1 
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q q'  * ' 

t m-t  -m^ 


q q'  £ 
s n-s  -n 


y pij  ( hqi  hq'J  h- 

^ \ \ Its  l,m-t,n-s  2ts"2,m-t,n-s 

i.j  = l 


^ h?’j 


(27) 


£k 


p, 


2mn 


t 

z 

q =-t 


2q* 


Rel>  (w  ) - p ’ I ml)  (w  ) 
qm  k-1 ’ 'lqn  qm  k-1 


lqn  qm 


^ L 

v X,,.  , - (£)  R*  Vh^*  + Z 

10, LI  . . . k k 2mn  , _ 

i,j-l  q,q’=0 


z 


-q<t,s<q 
q-q'|<£<q+q'  m-q'<t<q'+m 

n-q ' <s<q ' +n 


(-D 


m-n+1  2£*1 


q q’  t 


t m-t  -jo 


q q'  £ 


s n-s  -n 


£ 

i J = 1 


h^1  h1^  t h^'j  „ 

2ts  l,m-t,n-s  Its  2, m-t, n-s 


(28) 


IV.  Star  Tracker  F.quations  in  Terms  of  Harmonics 

A star  tracker  is  a telescope-detector  system  mounted  with  two  degrees 
of  freedom  to  the  satellite.  The  field  of  view  of  the  detector  is  a 8 by  8 
degree  window  whose  center  point  is  on  the  optical  axis  and  whose  plane  is 
norma]  to  this  axis.  A star  appearing  within  this  window  is  sensed  by  the 
detector  and,  after  some  electronic  processing,  results  in  the  output  of  two 
voltages  representing  the  position  of  the  star  within  the  window  along  two 
mutually  perpendicular  axes. 

When  a star  tracker  observation  is  taken  at  time  k,  the  star  is  identic 

1 

fied  from  a star  map  and  its  apparent  direction  cosines  a(k)  = la^ (k)  , a200»a300] 

with  respect  to  the  earth-centered  inertial  set  of  coordinate  system  can  be  easily 

computed  from  the  absolute  velocity  of  the  satellite.  The  apparent  star  direction 

T 

cosines  u(k)  = [Uj (k) ,u2(k) ,u3(k)]  with  respect  to  the  tracker  base  reference 
axes  are  related  to  a as  follows: 


- I?  - 


u(k)  = A t A j (k)a(k) 
x x xz 


(29) 


where  A is  the  constant  3x3  orthogonal  matrix  representing  the  orienta- 
xx 

tion  of  the  tracker  base  reference  axes  with  respect  to  the  satellite  base 

reference  axes  and  A j(k)  is  the  3x3  orthogonal  matrix  representing  the 

xz 

satellite  attitude  with  respect  to  the  earth-centered  inertial  set  at  time 
k. 

The  mathematical  model  used  [1]  and  [2]  for  the  relationship  between 
the  tracker  output  voltages  [y  (1)  , y^(k)]  and  u(k)  is  the  following: 


yjOO 

-Uj (k)/u2 (k) 

Hj  (k) 

1 

*< 

1 

u3(k)/u2(k) 

— • — 

-f 

n3(k) 

_ -t 

(30) 


With  [nj(k),  n„(k)]  denoting  two  independent  white  Gaussian  noise,  the  model 
is  only  an  approximation.  It  was  pointed  out  by  E.  J.  Lefferts  of  the  GSFC/ 
NASA  that  the  following  model  provides  just  as  good  an  approximation  to  the 
relationship  between  fyj(k),  y3(k)]  and  u(k)  : 


v,  (k) 
y3(k) 


"Uj (k) 

u3(k) 


(k) 

n3(k) 


(31) 


It  will  be  seen  later  that  the  components  of  A j are  simple  linear  com- 

xz1 

binations  of  the  rotational  hermonics  of  order  1.  Whence  so  are  the  components 

of  u(k).  Using  the  model  (31),  we  do  not  need  to  use  the  set  of  Fortran  IV 

l 

programs  provided  in  the  Appendix  to  compute  h^fkj's  in  (7)  to  approximate 
h(sk,k).  Above  all,  the  star  observed  by  the  star  tracker  changes  from  time 
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to  time  and  thus  h(s^,k)  is  a time-varing  function.  Using  the  Fortran 
programs  to  compute  h (k)'s  for  each  star  observation  may  amount  to 
unbearable  computational  burden,  especially  when  the  estimation  has  to 
be  done  on  board  the  satellite.  The  use  of  the  model  (31)  not  only 
eliminates  such  a difficulty  but  also  keeps  minimal  the  number  of 
harmonics  required  to  update  the  conditional  densities  of  the  sat- 
ellite attitude.  In  the  following,  we  will  restrict  our  attention 
to  the  model  (31). 


The  SAMU  uses  two  star  trackers.  We  will  use  a single  underline 
and  a double  underline  to  refer  to  Star  Tracker  I and  Star  Tracker  II, 
respectively,  e.g.,  (u^ , u^,  u^)  denotes  the  apparent  star  direction 
cosines  in  the  Star  Tracker  II  base  reference  axes  (5T|,x^,x^2) . When 
the  underlines  are  omitted,  the  equations  are  valid  for  both  star  trackers. 


We  pool  the  measurements  from  the  two  star  trackers  and  write 


1 

rak 

h1(sk,k) 

1 

•“H 

> 

1 

2 

mk 

h2(sk,k) 

2 

Vk 

3 

= = h(sk,k)  + vk  = 

3 

+ 

3 

\ 

h (sk,k) 

Vk 

l 

"’s'* 

1 

h4(sk,k) 

1 

'T  .* 

> 

1 

where 


h1(sk,k) 

-UjCSj^.k) 

h2(sk,k) 

H3(sk»k) 

h3(sk,k) 

-Ui(VIO 

h4(sk,k) 

u3(sk,k) 

(32) 


(33) 


- 19  - 


We  will  now  express  ik  as  a linear  combination  of  the  rotational 
harmonics  of  order  1:  We  recall  that 


min(f-n,f+m)  - 

/ fn,  = V , nt  [(l+m)!(£-»n)!(l+n)!(;l-n);;r 

mir  , 1 ' (l+m-t) ! (t+n-m)  !t!  (£-n-t) ! 

t-max(0,m-n)  1 J J ^ 1 


2f*-m-n-2t  0 . 2t+n-m  6 


U n(<t>,0.<l')  = exp  [ (*  - (ji)m  - {-  + i|>)n]  (6) 


whence 


!i 

Re0!io 

ROD-ll 

'-10 

Doo 

ReDoi 

1 

-11 

>"Dil 

Reojj 

-isin(^+i^)  (l  + cosB)  ^imjisine  -cos ((j>-i|;)  (l-cos0) 


^osijisinO 

-4sin((j>-ip)  (l-obs0)  -/=Cos\Jisin0  ^cos  (l+cos0) 
Z >£.  Z 


A 

y^sini^sine 


The  matrix  A can  then  be  expressed  in  terms  of  the  harmonics  as  follows: 


cos<|>cos<Ji-s  in<f>cos6sin\|>  cos^sin^+simpcosijicoso  sin<j>sin0 

-simj>cos<j>-cosijtsin<|>cos0  -sin<t>sinij>+cos<{>cos\jicos0  cos<|isin0 

j sin$sin0  -cos$sin0  cose 


ReD^-ReD^ 
ImD*  jj  + Imojj 


/2RcD 


ReD^+ReD^j  j 

/2ImD!io 


-/zReDj, 

-/2ImDL 
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Let  the  ixj-th  component  of  the  constant  matrix  A be  denoted  by  m. . . 

xx 


From  (29),  it  follows  that,  for  j = 1,2,3  and  for  both  star  trackers. 


Uj(k)  = »j3a3(k)D00(sk)4| 


*/2"'j3al{k)ReP-10(sk)  + (mj2a2(k)'n,jlal(,c)ReD-ll^sk) 


-/2m^1a3(k)ReDQ]  (sk)+ ^ (k)+nu  2a2(k))ReD^ j (sk) 


/2m_j3a2(k)ImD^1(sk)  - (hu  ^(kj+m..  2a1  (k) ImD*n  (sR) 


v^m^2a3(k)ImD^1(sk)  + (nu  ^ (k)  -nu  ^(k)  ) XmD^  (sR) 


(37) 


The  Fourier  coefficients,  J^^k)'s  and  h^*^k)'s  for  h1(sk>k)  can  be  easily 
identified  from  (33)  and  (37)  and  will  not  be  displayed  here.  We  only 
note  that  they  can  be  very  easily  determined  from  the  star  direction  a(k) 
and  the  recursive  formulas  are  then  readily  applicable. 


V.  Updating  the  Conditional  Densities  Using  Star  Tracker  Measurements 

We  *111  now  display  the  recursive  formulas  for  the  conditional  densi- 
ties of  the  satellite  to  be  used  in  the  next  two  modules  of  this  project.. 
The  star  tracker  equations  are  (32),  (33),  and  (37)  developed  in  the  pre- 
vious section.  We  assume  that  the  measurement  noise  process  vk  has  statis- 
tically independent  components,  i.e.,  RkJ=0,  if  i^j . 

We  recall  that  the  function  h1  in  (33)  is  a linear  combination  of 

the  first  order  harmonics,  ReD*  and  ImD*  . Hence  h?*  =h~*  =0  unless 

mn  mn  lmn  2mn 

q=l . This  observation  greatly  simplifies  the  multiple  summation  sign  in 


T 


I 

I 

I 

I 

I 

I 

I 

I 

I 

I 

I 

I 

! 

I 

r 

! 

i 

i 
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front  of  the  brackets  in  (27)  and  (28): 


E 


E 


q,q’=0 

I q-q* 


- q<t,s<q 
tn-q ' <t<m+q ' 

"-q'is£n+q ' 


Z 

max(-l  ,m-l)<t<jnin(l  ,m+l) 
max(-l ,n-l)<s<min(l ,n+l) 


In  view  of  the  assumption  that  R^=0,  for  ij*j,  we  obtain  from  (27)  and 
(28)  the  following  recursive  formulas:  for  £=1,  2 and  m,n  = ...  , t, 


tk  V £.k-l_  tX  , -1  * £,k-l  JL  , -1  , 

Pi  = L,  p ' ReD  (w,  ,)  + p_  ImD  (w,  ,) 
^lmn  _ „ 1 lqn  qm  k-1  r2qn  qmv  k-1' 


q=-f 


2qn  qm' 


* *[o.i.i . ,,  ,, 

1 1 j = l max(-l  ,m-l)<t<min(l  ,m+l) 

max  (-1  ,n-l  ).<s<_min  (1  ,n+l ) 


(-D 


m-n+1 


2&*  I 
4 


' 1 1 1 l \ 

t m-t  -m 


1 1 l 


c n e „ , . , k I Its  1 ,m-t,n-s 
s n-s  -n  / j=l  1 ’ 


-h!J  hlj  , 

2ts  2, m-t, n-s 


J 


(38) 


* / 

Ik  y £,k-in  nl  , -l  , 

p0  = L>  Pt  ReD  (w,  ,) 
r2mn  ^ i 1 2qm  qm  k-1' 


£,k-lT  nl  , -1  ,, 
p,  ImD  (w.  , ) 
*iqn  qm  k-1  1 


Mm.iW  S R^mlhiL  + 


[0,  L] K , k V 2mn  , . M ... 

1 1 j=l  max(-l  ,m-l)£t£min(l,m+l) 


(-D 


m-n+1 


2£+0 

l4  ( 

1 1 l 

t m-t  -mj 


max(-l ,n-l)<s<nin(l ,n+l) 

1 1 l 


4 

_ i . . k 

s n-s  -n  j j = l 


. hjj  , 

\ 2ts  1, m-t, n-s 


. h'j  h!j  . 

Its  2, m-t, n-s 


i 


1 


(39) 
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where,  from  (19), 


(' 

1 i \ 

| = (-l)1+m_t 

ill!  U- m)  ! (£+m)  ! 

l. 

m-t  -in  j 

(3+£) ! (1+t) ! (1-t) ! (1+m-t) ! (1-m+t) ! 

m i n ( (' -m  ,t ) 

(-l)r  (^+1  -m+t-r)  ! (1+m-t+r) ! 

rr-max(0, -m)  (£-m-r)  ! (r+m)  !r!  (£-r)  ! 


(40) 


The  conditional  density  p(s^/m  ) is  then  obtained  through  (26): 


p(s,  |m  ) = exp 


2 

V 


f = 0 


1 £k  JL  , . V I Ik  „ rX  , , £k  n£ 

2^  1 00D00  Sk  + L „ ! PlmnReDmn(sk)  + P2mnImDmn  (sk>l 


m,n=-£ 

m<n 

Of  m=n>0 


(26) 


1 


1 Ok 

2 P i oo  = t*ie  norma  1 i z. i r»^  constant 


tk  £k 

It  is  noted  that  there  are  35  Fourier  coefficients,  p,  and  p_  , which  are 

lmn  2mn 


recursively  computed  by  reasonably  simple  scalar  equations  (38)  and  (39), 


II.  Nonobservability  of  the  Star  Tracker  Equations 

We  consider  the  mathematical  model  (31),  to  be  repeated  in  the  fol- 
lowing, for  the  star  tracker  measurement  in  this  section.  Our  argument 
here  applies  also  to  the  model  (30).  We  recall  that  the  model  (31)  is 


V,(k)' 

-Uj  (sk,k) 

n j (k) 

= 

+ 

/3(k). 

_ u3(sk’k). 

_n3 (k)  _ 

(31) 


u(sk,k)  = A t A j (k) a (k) 

X X xz 


(29) 


where  a(k)  is  the  apparent  direction  cosines  of  the  observed  star  with  respect 
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to  the  earth-centered  inertial  set  z of  coordinate  system,  A is  the 

x x 

constant  3x5  orthogonal  matrix  representing  the  orientation  of  the  tracker 

base  reference  axes  x*  with  respect  to  the  satellite  base  reference  axes  x, 

and  A (k)  is  the  3x5  orthogonal  matrix  representing  the  satellite  attitude 
xz 

with  respect  to  the  earth-centered  inertial  set  at  time  k. 

The  SAMU  uses  two  star  trackers.  We  will  use  a single  underline  and 
a double  underline  to  refer  to  Star  Tracker  1 and  Star  Tracker  II,  respective- 
ly. When  the  underlines  are  omitted,  the  equations  are  valid  for  both  star 
t rackers . 

We  pool  the  measurements  from  the  two  star  trackers  and  write,  suppressing 
the  time  variable  k, 


1 " 
m 

h1  (s) 

r 

V 

2 

m 

h2(s) 

2 

V 

3 

m 

= m = h ( s ) + v = 

h3(s) 

+ 

3 

V 

4 

m 

h4  (s) 

4 

V 

■ . 

where 


h 1 (s) 

-Uj (S) 

ir  (s) 

u3(s) 

h3(s) 

-Ui(s) 

v 

X. 

i 

. =3  ^s) . 

(32) 


(33) 


We  say  that  the  measurements  m are  not  observable,  meaning  that  there 
are  two  different  states,  s and  f,  that  the  measurements  m can  not  distinguish, 
i.e.,  h(s)  = h(f).  This  nonobservability  is  essentially  caused  by  the  lack 
of  information  about  u^(s)  in  the  model  (31). 
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The  vector  u(s)  is  the  apparent  direction  cosines  of  the  observed 
star  with  respect  to  the  tracker  base  reference  axes  and  its  second  com- 
ponent l^Cs)  is  either  positive  or  negative  /l  - u^(s)  - f s)  , F:urther 
more,  the  star  tracker  looks  in  the  positive  direction  of  u2(s).  Hence, 
u9(s)  = /l  - u“(s)  - u~(s)  and  ^(s)  is  completely  determined  by  u^(s) 
and  iijts),  which  are  directly  measured  in  (31).  Indeed,  the  physical 
system  that  (32)  and  (53)  try  to  describe  is  observable.  But  the  direc- 
tion of  the  star  tracker  is  by  no  means  taken  into  consideration  in  the 
model  (31)  (nor  in  (30)).  We  will  now  illustrate  this  point  with  the  fol 
lowing  example. 


Example.  Assume  that 


A 

x x 


A 

x x 


1 0 0 

0 1 0 

0 0 1 

1 0 0 

0 0 1 

0-1  0 
1 0 0 


s 


=A 


XZ 


I 


0 1 0 
0 0 1 


1 0 0 


f=A 

xz 


I 


0-10 
0 0 -1  _ 


a = 


a = 


" 

0 

1 

0 

0 

0 

1 


IVe  note  »hat  this  example  is  also  valid  for  the  model  (30)  for  which 
h(s)  = [ - J j (s)/u^(s) ,u3(s)/u2(s) ,-Uj (s)/u2(s) , u3 (s) /u2 (s) ] T. 

As  the  star  trackers  look  in  the  positive  direction,  the  direction 

j 

i 

( 

i 

t 


cosines  u(f),  of  which  the  second  component  is  negative,  can  not  be  those 
of  the  observed  star.  They  only  form  a pseudo-image  of  the  observed  star. 
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which  the  measurements  (32)  and  (33)  can  not  distinguish  from  the  real 
image  of  the  same  star. 


VI 1.  Remedying  the  Nonobservability 

We  recall  that  the  conditional  density  p(m^|s^)  is  used  in  calculating 
pfs^  | ) and  that 


p K, 


|sk)  = (2ti) 


(detR.  ) exp 


I (m^-h1  (sk,k))  (m^-h^  (sk,k))J 
» ,j  = 1 


(15) 


where  r is  the  dimension  of  the  measurement  vector  m.  From  this  equation, 
it  is  clear  that  p(mk|sk)  takes  on  the  same  value  at  the  state  s and  at  the 
pseudo-state  f resulting  respectively  from  the  images  u(s)  and  the  pseudo-images 
u(f)  of  the  observed  stars,  since  h(f)  = h(s). 


In  order  to  eliminate  such  effect  of  the  star  pseudo- images , a new  ap- 
proach, which  we  call  the  pseudo-measurement  technique,  was  developed.  The 
idea  is  pretty  simple.  Supposing  that  u and  were  directly  measured  and 
(32)  and  (33)  became 


1 

m 

hA(s) 

1 ' 
v 

2 

m“ 

h2(s) 

2 

V 

3 
m 

4 
m 

= m = h(s)  + v = 

h3(s) 

h4(s) 

+ 

3 

V 

v4 

5 

m 

h5(s) 

V5 

6 

m 

_ h6(sl 

V6 

(41) 
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and 


h1  (s) 
h2(s) 
h3(s) 
h4(s) 
h5(s) 
h6(s) 


-Uj (s) 

U (s) 

-u2 (s) 

y3(s) 

u2(s) 

y2  (s) 


(42) 


the  equation  (15)  would  remain  unchanged  with  r = 6 instead  of  4.  Of  course, 
and  are  not  really  available.  However,  if  the  estimate  s^  j of  s^  ^ is 
good,  then  u^(w,  } j)  and  u,(w^  j0s^  j)  should  be  equally  good  estimates  of 

mji  and  m^.  It  seems  only  natural  to  use  ij_2  ^wk- 1 oSk-l  ^ ancl  -2^wk  l“*k  P aS 
.•a su roments  of  i^C-Sj.)  and  u2(s^),  respectively. 


me 


Now  setting 


5 

v = 

- i2<v 

II 

\G 

> 

^Vi^k-i* 

- y3Csk) 

5 

A 

m = 

u-2(wk-rSk-l5 

6 

m = 

(43) 


we  retrieve  exactly  (42)  and  (43).  Here,  the  question  is  whether  we  still 
have  (15)  with  r=6.  This  obviously  depends  on  the  conditional  sampling  dis- 
tributions of  v5  and  in  (44)  given  the  state  s^  at  time  k which  unfortu- 
nately remains  unclear  as  of  today.  We  note  only  that  j is  a measurable 
function  of  m^'1  = (nij,  ...,  and  hence  v^(k)  and  v6(k)  are  conditional- 

ly independent  of  v1  (k),  i = l,  ...,  4. 

We  recall  that  the  nonobservability  problem  arises  from  the  missing 
sign  of  u2<  Perhaps  we  do  not  need  all  that  much  information  about  u2  as 
might  be  possible  to  obtain  from  a precise  expression  of  the  conditional 
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sampling  distributions  of  v5  and  v,(',  as  long  as  the  effect  of  the  pseudo- 
images of  the  observed  stars  can  he  eliminated  without  affecting  (15)  with 
r=4  . This  can  indeed  be  achieved  by  assuming  that  tiie  conditional  sampling 
distributions  of  v^  and  v6  are  normal.  First  of  all,  the  equation  (15)  is 
valid  again  with  r=6  under  this  assumption.  Secondly,  by  setting  R55  and 
R66  equal  to  4,  we  assign  a probability  of  95%  to  a positive  value  of  U2, 
leaving  the  equation  (15)  with  r=4  relatively  intact.  Thirdly,  in  view  of 
the  central  limit  theorem,  the  assumption  may  not  be  hard  to  swallow  after 
all. 

VI I L An  F.stimation  F.rror  Criterion  and  the  Optimal  Estimate 

In  order  to  define  an  error  criterion  for  orientation  estimation,  it 
is  necessary  to  have  a measure  of  the  distance  between  two  orientations. 

We  will  first  describe  such  a measure,  using  quaternions.  We  recall  that 
a rotation  about  an  axis  in  the  direction  of  a unit  vector  [£,m,n]T  through 
an  angle  <J>  is  represented  by  the  (unit)  quaternion 

q = [q1,q2,q3,q4]  = [«m  f,  msin  f,  nsm  cos  f] 


Given  two  orientations,  the  minimal  angle  in  radians  required  to  bring 

one  into  the  other  is  a natural  measure  of  distance  between  them  and  defines 

a Riemannian  metric  on  S0(3).  If  the  orientations  are  represented  by  the 

quaternions,  q and  p,  and  the  minimal  angle  is  denoted  by  p(q,p),  then  we 

have  q^p  = cos  ~ p(q,p).  As  y (1  - cos  p)  is  a monotone  increasing  function 

of  p , a measure  of  distance  between  p and  q can  be  defined  to  be  j (1  - 

2 

cos  p (q,p))  = 1 - (qTp)  • It  can  be  shown  that  if  the  orientations,  q and 
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p,  are  described  by  the  3x3-dimensional  orthogonal  matrices,  Q and  P,  then 
this  measure  of  distance  can  also  be  expressed  as  | {3  - tr  PQT). 

We  are  now  ready  to  define  the  error  criterion  for  orientation  estimation. 
Let  q be  a random  quaternion  and  p its  estimate.  Then  a measure  of  the  esti- 
mation error  is 


J(q.P)  = E(1  - (qTp) 2 ) . (44) 

If  the  probability  distribution  of  q is  given,  the  estimate  p which 
minimizes  J may  be  obtained  from  observing  that  j(q,p)  * i - pT  E(qqT)p  • 

It  is  well-known  that  the  quadratic  form  p* Vp  of  the  positive  definite 
matrix  V = E(qq'f)  is  maximized  when  p is  the  unit  eigenvector  associated 
with  the  largest  eigenvalue  A of  V.  Moreover,  the  maximum  value  is  A. 

Hence, 

min  J(q,p)  = 1 - qTE(qqT)q 
P 

* i - A 

where  A = the  maximum  eigenvalue  of  E(qqT ) 

q = the  unit  eigenvector  of  E(qqT) 
associated  with  A . 

The  probability  distributions  on  S0(3)  are  expressed  in  terms  of  the 
Euler  angles  (<fr,0,'IO  in  (26).  The  following  relationships  between  the 
quaternion  components  and  the  Euler  angles  will  have  to  be  used  to  calcu- 
late the  optimal  estimate  q and  its  estimation  error  1 - A: 


6 $ -*ti 

qj  = sin  y cos  *-£- 


^2 


= sin 


9 


sin 


q3  = cos  j sin  -j 


0 

cos  j . — — 
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IX.  Simulation  Results 

Three  simulated  examples  with  various  system  parameters  and  noise  levels 
are  given  in  this  section.  They  were  chosen  to  test  the  robustness  of  the 
optimal  scheme  ami  therefore  represent  tougher  working  conditions  than  the 
real  ones.  The  measurement  noise,  v1  in  (42),  is  set  at  two  different  levels 
to  illustrate  how  the  noise  level  can  affect  the  estimator  performance.  The 
variance,  1/R11,  of  v*  is  1/36  for  A part  of  the  examples,  and  is  10~4  for  B 
part  of  the  examples.  The  reader  is  rerferred  to  the  Module  II  report  for 
the  specification  of  all  the  system  parameters. 

The  four  graphs  of  the  A part  of  the  i—  example  are,  respectively, 

GRAPH  (i.  A,  1): 

______  the  error,  ERH1,  of  the  optimal  estimate  obtained  by 

local  integration 

-------  the  error,  ERH2,  of  the  maximum  likelihood  estimate 

GRAPH  (i.  A,  2): 

The  same  as  GRAPH  (i.  A,  1)  except  that  the  vertical  scale  is  changed 
and  the  points  at  k = 1 and  2 are  removed  to  magnify  the  vertical  variations. 

GRAPH  (i.  A,  3):  ' 

the  difference,  ERH1  - ERH2,  of  the  errors 

the  distance  between  the  maximum  likelihood  estimate 
and  the  optimal  estimate  obtained  by  local  integration 

i 

> 

GRAPH  (i,  A,  4): 

\ 

The  same  as  GRAPH  (i.  A,  3)  except  that  the  vertical  scale  is  changed 
and  the  points  at  k = 1 and  2 are  removed  to  magnify  the  vertical  variations.  j 

1 


I 

I 

I 
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The  symbols  to  be  used  to  specify  the  system  parameters  and  noise 
levels  are  defined  as  follows: 

1.  The  attitude  propagation  equation  is 

sk+i ^k+i ’ °k+i*  *k+i}  = WW  0 WW  (3) 


2.  The  initial  attitude  at  k = 0 is  represented  by  a quaternion  s()  = [q^q^ 
Vq4]I- 

3.  The  initial  distribution  of  sQ  has  a rotational  normal  density 


p(s  ) = c exp  [a  ( i b.q.  ) ] 

u 1-1  1 x 


4.  The  Euler  angles  o^,  Bk>  Yk  are  obtained  from  multiplying  the  constants 
C1 1 C2 * C3»  resP->  ^ pseudo-random  numbers  from  the  uniform  distribu- 
tion on  the  interval  [0,  1]. 

5.  The  star  tracker  measurement  and  the  pseudo -measurement  equations  are 


I 

I 

I 

I 


where 


mj.  = h1^,  K)  + vk>  i = 1»  • • • • 6 

^ -\  6 

P(v)  = (2tt)‘3  (I  R11  ) exp  l R 

i=1  i=l 


(42) 


ii,  i.2 
(v  ) ) 


6.  The  apparent  direction  cosines  u with  respect  to  a star  tracker  and  those 
a with  respect  to  the  earth  are  related  by 


u(k)  = A t A j (k)  a(k) 
x x xz 


(29) 
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GRAPH  (1,  A,  3) 


GRAPH  (1,  A,  4) 
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GRAPH  (2,  B,  4) 
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Remark.  Let  us  recall  the  nonohservabi 1 ity  example  given  in  Section  II. 

The  pseudo- state  of  the  real  state  s0  is  exactly  [b. , b_,  b_,b.],  which 

^ v ^ 

is  chosen  to  be  the  mode  of  the  initial  density  of  sQ.  The  pseudo-measure- 
ment technique  thus  picks  wrongly  the  pseudo-state  at  the  first  time-point. 
This  example  was  in  fact  delivcrately  designed  to  work  against  the  pseudo- 
measurement  technique.  The  simulation  results  to  be  depicted  in  the  fol- 
lowing graphs  indicate  without  doubt  that  the  pseudo-measurement  technique 
has  unbelievable  sel f-corrccting  power  and  switches  to  the  real  images  of 
the  stars  very  quickly  (at  the  second  time-point  in  this  example). 
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III.  Comparison  Between  the  Maximum  Likelihood  Estimates  and  the  K-B  Estimates 

We  note  again  that  it  takes  much  more  CPU  time  to  compute  the  optimal 
estimates  even  by  local  integration  and,  based  on  the  simulation  results,  the 
local  integration  estimates  are  not  much  better  than  the  maximum  likelihood 
estimates.  Therefore,  a comparison  was  made  between  the  K-B  estimation  and 
the  maximum  likelihood  estimation  instead  of  the  optimal  estimation. 

The  comparison  was  conducted  by  E.  J.  Lefferts  of  the  GSFC/NASA.  A.  N. 
Mansfield  of  the  CSTA  generated  sequences  of  33  star  tracker  observations. 

The  average  body  angular  velocity  was  provided  every  one  third  of  a second, 
and  the  tracker  observation  was  taken  every  two  minutes.  The  standard 
deviation  of  the  tracker  measurement  noises  is  20  arcseconds. 
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For  such  a low  noise  level,  it  is  known  that  linearization  is  a very 
good  approximation  and  the  extened  K-B  filter  is  expected  to  be  near  optimal 
Indeed,  our  comparison  results  confirmed  these  long-standing  conjectures. 

Two  typical  examples  were  included  in  the  following.  We  note  that  the  maxi- 
mum likelihood  estimates  are  almost  always  better  and  converge  faster  than 
the  K-B  estimates.  However  they  are  close,  especially  in  the  steady  states. 


The  system  parameters  and  the  noise  statistics  for  the  two  comparison 
runs  to  be  reported  in  the  following  will  now  be  given,  using  the  symbols 
established  in  Section  IV  of  the  Module  II  report: 
a = 2 * (180/2tt)  2 

Rix  = [20  * (1/3600)  * (it/180)]  "2 , i=l,  2,  3,  4 
R**  = 0,  i = 5 , 6 


A 


t 

x x 


t 

x x 


1 0 0 

0 1 0 

0 0 1 

1 0 0 

0 0-1 

0 10 


The  ^our  graphs  of  the 


example  are, 


respectively. 


GRAPH  (i,  1): 

the  angular  error  in  arcseconds  of  the  K-B  estimate 

the  angular  error  in  arcseconds  of  the  maximum  likelihood 

estimate 


GRAPH  (i,  2): 

The  same  as  GRAPH  (i,  1)  except  that  the  vertical  scale  is  changed  and 
the  points  at  k*l,  ...,  4 are  removed  to  magnify  the  vertical  variations. 


- 41  - 


I 

| GRAPH  (i,  3): 

the  angular  error  of  the  K-B  estimate  minus  that  of  the 
|j  maximum  likelihood  estimate 

- GRAPH  (i,  4): 

■ The  same  as  GRAPH  (i,  3)  except  that  the  vertical  scale  is  changed  and 

I the  points  at  k=l 4 are  removed  to  magnify  the  vertical  variations. 
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GRAPH  (4,  3) 


GRAPH  (4,  4) 


EXAMPLE  5 


GRAPH  (5,  3)  GRAPH  (5,  4) 

EXAMPLE  6. 
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APPENDIX 


M05SO3 


Main  Program 


Purpose : 


lo  obtain  the  optimal  estimate  s^,  maximum  liklihood  estimate 
s of  the  satellite  attitude  s^  satisfying 

‘VVW  = wk(ak'gk'7k)  ° sk-l^k-l'  0k-l'  ^k-l5, 
using  the  measurements  (m^ , m.,,  ...  m^}  acquired  from  two  star 

trackers,  which  are  described  by 

\ = h(V  K)  + Vk 


Common  variables: 


/ B L K 1 / N,  NZ,  MT 

/ B1.K2/  PA1  2 , 11(9 , H,  1>,  AXX 

/ BI.K3/  AD,  AA,  BB,  W(J,  OBN,  P0,  Q0,  PI,  Q1  , 

/ BLK4/  AK,  QS,  DIR,  QW,  RR , SINE,  COSN,  QSN,  QH1 , QH2 , QH(6 , ERRN, 

ERH1 , ERM2 , ER12 


Input 


number  of  subintervals  in  [0,  2tt ] 

number  of  subintervals  in  [0,  2tr]  for  the  first  and 
third  Euler  angles  4> , ip 

number  of  subinteryals  in  [0,t]  for  the  second  Euler 
angle  0 

mesh  for  integration  (=  2^/N) 

mesh  for  integration  (=  2u/NZ) 

mesh  for  integration  (=  tt/MT) 


I 

I 

I 

I 

I 

I 


j 

j 


j 


i 

J 
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M-2 


AXX  2x3x3  dimensional  array  representing  each  orientation 

of  the  star  tracker  base  reference  axes  with  respect 
to  the  satellite  base  reference  axes. 


AD  2x3  - dimensional  array  representing  the  apparent 

direction  cosines  of  a star  observed  by  each  of  the 
two  star  trackers  with  respect  to  the  earth-centered 
inertial  set  of  coordinate  system. 


W0 

QW 

P0,  Q0 


AK 

AA,  BB 

RR 


satellite  attitude  propagation,  Euler  angles,  3-dim. 
array. 

quaternion  form  of  W0,  4-dim.  array. 

coefficients  of  harmonics  in  P(s,  | m^  , which 
will  be  updated  for  P(s^jm^)  to  Pi,  Q1 , 2x5x5-dim. 
arrays . 


parameter  of  the  rotational  normal  distribution  of 
the  initial  satellite  attitude  SD. 


coefficients  of  {ReDifs,)}  , (imD  mn(Sk)}  in 
function  h(s^,K),  6x3x3-dim.  arrays. 


6 -dim. 


diagonal  elements  of  the  covariance  matrix  for  the 
normal  distribution  of  the  observation  noise,  6-dim. 
array. 


SINE,  COSN 
SDD1 , SDD2 


table  of  sir.  <p , cos  4>,  33-dim.  a'rrays. 


(dVfQ)  } , { d * 
f.  rnn  p ^ inn 
for  £=1,2  , 


(8)}  for  ReDf  (<J>,e,<J0, 

3x3x17  and  5x5xl7-dim. 


ImD  (<J>,0  4/  ) 
mi  Y’ 
arrays . 


QS 

QH0 

1X0, IY0 


attitude  s^  ^ , 4-dim.  array. 

(k-l)th  stage  maximum  liklihood  estimate  of  s^  ^ to 
generate  a predicted  estimate  of  s^,  4-dim.  array. 

initial  random  integers  which  create  measurement 
noise,  satellite  attitude  propagation,  and  AD. 


Output 

OBN  star  tracker  measurements,  6-dim.  array. 

PI ,Q1  updated  coefficients  of  the  harmonics  in  the  con- 

ditional density  P(s,  jm^),  2x5x5-dim.  arrays, 
input  for  the  (k+l)tn  stage. 


I 

[ 
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QS1 

attitude  Sj,,  4-dim.  array,  renamed  as  QS. 

QSP 

predicted  estimate  of  s^,  generated  by  W0 
4-dim.  array. 

and  QII0, 

Sill 

estimate  s^  of  the  satellite  attitude  s^, 
array . 

4 -dim. 

QH1 

sequence  of  { s^  } , 4x50-dim.  array. 

SH2 

A 

A 

maximum  likelihood  estimate  s,  of  s,  . 

k k 

QH2 

sequence  of  {s^}  , 4x50-dim.  array. 

ER1 

estimation  error  of  the  attitude  estimate 

Sk  ‘ 

ERRN 

sequence  of  estimation  errors,  50-dim.  array. 

ER2 

A A 

distance  between  s^  and  s^,  P(s^»  s^) . 

ERH1 

A 

sequence  of  {p(s,  , s.)}  , 50-dim.  array. 

K X 

ER3 

A A 

distance  between  s^  and  s^,  Pfs^,  s^) . 

F.RH2 

A 

sequence  of  {p(c^,  s^)  , 50-dim.  array. 

A A 

A A A A 

ER4 

distance  between  s^  and  s^,  P(s^,  s^) . 

ER1 2 

/A 

sequence  of  {p(s^,  s^)}  , 50-dim.  array. 

Special  Considerations: 

i.  Parameter  AD  is  generated  at  each  stage  k from  s^  considering 

the  deviation  from  the  value  a in  the  equation. 


0 

1 

0 


= A -A  (s. )u 
xx  xzv 


2. 


this  is  done  in  'ADC' . 

Attitude  propagation  W0  is  generated  at  each  stage  in  'NEW0'. 
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3.  Measurement  OBN  consists  of  six  components , which  are 

OBN(i)  = FU(i , DD,  AA,  BB)  ♦ V(i)  1 < i 4 

OBN(i)  = FU(i , DD,  AA,  BB)  5 < i < 6 

For  1 5 i £ 4,  DD  represents  { ReD*  (s.  ) },  { ImD1  (s,  )} 

’ r mn  k ’ mn  k 

For  5 £ i £ 6,  DD  represents  { ReD1  (s.  ) },  { I D1  (s. ) } , 

r mnv  k ' m mn  k ' 

* * 

where  called  predicted  estimate  of  s^ . 

In  this  sense,  OBN  is  called  pseudomeasurements . 

4.  The  initial  distribution  p(sD)  is  assumed  to  be  a 
rotational  normal  distribution  on  S0(3)  with  parameter  AK. 

5.  In  order  to  reduce  integration  time,  32  subintervals  for 
<j) , 4/  and  16  subintervals  for  0 are  used  in  the  trapezoidal 
rule,  where  0 £ <p,ip  £ 2tt,  0 5 0 5 1 

6.  In  the  computation  of  optimal  estimate  and  estimation 

1 2 

error,  tables  of  sin<)>,  cos<j>,  d (0) , d (0)  are  used  instead 

mn  mn 

of  calling  subprograms. 

7.  Integer  KK  indicates  which  stage  should  be  updated  and  MM 
shows  at  (MM-1)  stages,  the  values  QSN,  QH1 , QH2,  ERRN,  ERH1 , 
ERH2 , ER12.  Skipping  given  steps  JMP. 

8.  At  the  end  of  each  stage,  necessary  data  are  stored  in  the 
disk  space  and  will  be  read  at  the  beginning  of  RUN  by  TRANS. 

Subprograms  Called: 

INSDD,  TRANS,  SI NCOS,  QTN,  IN1TPQ,  RANDU,  NEWO,  QXQ,  GAUSS4 , 

DR  12,  ADC,  C0EFF1 , FU,  NPAQ,  ERR. 


-□ 


PRINTOUT 
if  ID=3 
(TRANS) 


New  attitude 


P1,Q1  from  P0,Qp 
(NPAQ) 


ADC-1 
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AH  C 

A Subprogram 
Purpose : 

To  choose  two  appropriate  sets  of  apparent  direction  cosines 
AD  with  respect  to  the  earth-centered  inertial  coordinate  system. 


Calling  Sequence: 

CAl.h  ADC  ( AXX  , DD,  VI,  V2  , ADI 


Common  Var i ab 1 es : 

None 

Input : 

AXX  2x3x3-dim.  array  representing  the  orientation  of  the 

two  sets  of  the  start  tracker  base  reference  axes 
with  respect  to  the  satellite  base  reference  axes, 
given  in  the  main  program,  A . 

x x 

DD  3x3-dim.  array  representing  { D1  (s,  ) } , computed  by 

' DR  1 2 ' . mn  k 

VI  ,V2  4-dim.  arrays  representing  random  numbers  from  the 

normal  distribution. 


Output : 

AD  2x3-dim.  array  representing  apparent  direction  cosines, 

a(k). 


Special  Considerations: 

1.  First  AD(=a)  is  chosen  to  satisfy  the  relation 


1 

0 


• a 


then  a is  replaced  by  a+v  and  normalized.  In  the  program, 

the  orthogonality  of  A ^ , A ^ is  used. 

xx  xz 
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v is  of  normal  distribution  with  a relatively  small  variance  for 
the  realistic  simulations. 
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BLK0 

A Subprogram 
Purpose: 


To  determine  a region  and  parameters  for  local  integration  adjusting 
a block  region  determined  by  'DMN0'. 


Calling  Sequence: 

CALL  BLK0  (A,  H,  Z,  T,  W,  NZ,  NT,  NW) 

Common  Variables: 

None 


Input : 

A 3-dim  array  representing  the  mode  of  the  conditional 

density  function. 

Z,  T,  W 2-dim  arrays  representing  the  coordinates  of  8 corner 

points  of  a rectangular  region. 

Output : 

Z,  T,  W 2-dim  arrays  containing  the  coordinates  of  a modified 

rectangular  region. 

NZ,  NT,  NW  integers  representing  the  numbers  of  meshes  in  Z,  T, 
W-directions , respectively. 


Special  Considerations: 

1.  The  numbers  of  mesh  in  Z,  T,  W-directions  are  adjusted  so  that 
they  approximately  become  12,  6,  12  or  6,  6,  6 according  to 
the  distribution. 

2.  Each  number  of  NZ,  NT,  NW  is  a multiple  of  6. 

Other  subprograms  called: 


None. 


Rough  estimate  of 
mesh  in  each  direction 

HI  = (Z(2)-Z(l))/12 

H2  = (T(2)-T(l))/6 

H3  = (W(2)-W(l))/12 


Uniform  mesh 

H = (max (HI , H2,  H3)  + min(Hl,  H2,  H3))/2 


1 

Number  of  mesh 
in  each  direction 

NZ,  NT,  NW 


i 

Coordinates  of  Z,  T,  W 
Z(l)  = A(l)-H- (NZ/2) 

Z (2)  = A(l)+H- (NZ/2) 
T(l)  = A(2)-H- (NT/2) 
T(2)  = A(2)+H- (NT/2) 
W(l)  = A(3)-H- (NW/2) 
W(2)  = A(3)+H- (NW/2) 
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COI.I  I 1 

A Subprogram 
I’u  rpose : 

To  sot  up  tables  of  the  coefficients  of  the  following  six 
functions  expressed  as  harmonic  series  on  S0(3), 

I ",  -"j  "v  y 2 1 1 

where  u.(k)  is  the  j direction  cosine  of  a star  tracker  at 
time  k and  is  given  by 

u.  (k  ) -iik  , • a^ (k)  • l)()0(sk ) + ( /2  m 3aj  (k)Rel)|]0(sk) 

+ (m j 2 ' a2 (k) -m . j • a j ( 1 ) ) ReDl j j (sk) - <^m. Ja3(k)Ren^1 (sk) 

+ (m. j -aj (k)+m^2-a2Ck))ReDj1(sk)] 

+ 1*^  mj3'a2(k')  ImD-io^Sk^‘^mjra2(k)  + mj2‘al^k^ImD-l^sk^ 

- ^ m^2-;*3(k)-lmDQi(sk)  + (nij2a1(k)  - mj  ] ’ a2  (k) ) lml)j } (sR) 

Calling  Sequence: 

CALI.  COliFFl  (AXX , AD,  AA,  BB) 

Common  Variables: 

None 


Input  : 


AXX 


Two  sets  of  star  tracker  base  reference  axes  with 
respect  to  the  satellite  base  reference  axes,  given 
in  the  main  program,  2x3x3  dimensional  array,  AXX(l,i,j) 
for  the  first  start  tracker  and  AXX(2,i,j)  for  the 
second,  mjj  in  the  equation  of  u.(k). 


I 

I 

I 

1 Output : 


RB 
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2x3  dimensional  array  representing  the  apparent 
direction  cosines  with  respect  to  the  earth-centered 
inertial  set  of  coordinate  system,  given  in  the  main 
program,  (k)  in  the  equation  u.(k). 


6x3x3  ^imensional  array  representing  the  coefficients 

of  ReD  (s,  ) in  -u, , u_,  -u, , u, . u_  and  u_ . 
mn  k'  —1—3  =1  =3  —2  =2 


6x3x3  ^imensional  array  representing  the  coefficients 

of  ImD  (s, ) in  the  six  functions, 
mn  k 


t 


Spec  ial  Cons iderat  i ons  : 


1.  The  coefficients  arc  arranged  and  stored  in  AA  and  BB. 
For  example,  the  coefficients  of  -u  are  stored  in  AA 
(1 , • , • ) as  follows 


-(m.iai+m.2a2) 

- /2m. _a,  - 
J 3 1 

Cj2»2 

AA  ( 1 , • 

,0  = 

-/2m. 

- 2m  a, 
j3  3 

/Im^a 

-(mj2a2-mjlal) 

/2m., a.  - 
33  1 

(miiai 

where  the  coeffic  i ents  of  ReD*  (s,  ) arc  placed  in  A ( 1 , m+2, 
n+2)  . 

The  arrangement  of  coefficients  are  m^de,  so  that  one  c^n 
express  u.(k)  as  an  expansion  of  {ReD  n(s,)}  and  (ImD^Cs^)} 
for  -1  < A,n  <.  1,  not  only  under  the  Condition  on  m,  n: 
m < n or  m=n  and  m ^ 0,  which  is  given  in  the  original  equa- 
tion u.(k).  Considering  the  fact  „ nl  , . , nn-m  ,1  , . 

J ReD  (s,  ) = (-l)  Red  (s,  ) 

. _ l 1 mn  * -m-n  k 

and  Tml)mn (s^)  = (- 1 )n  m+  ImD*m  n(sj.),  we  use  the  following 

matrix  form  to  see  the  sign  difference  between  the  elements 
(m+2,n+2)  and  (-m+2, -n+2). 


1 

1 

1 

-1 

1 

1 

-1 

1 

1 

for  the  real  part  and 

1 

0 

1 

1 

-1 

1 

-1 

1 

1 

r 


for  the  imaginary  part. 
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Other  Subprograms  Called: 


None . 


r 
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AA  ( 1 , 1 , 1 ) = (AXX  (k  , .1 , 1 ) • AD (K , 1 ) +AXX  (K , J , 2)  • AD (K , 2) ) • SGN 
AA ( I ,3,3) =AA (1,1 ,1) 

AA(I,1 ,2)»(/2.AXX(K,J,3) • AD (K , 1 ) ) • 3GN , AA (I ,3, 2) =-A ( I , 1 , 2) 

AA ( I , 1 ,3) = (AXX (K , J ,2) • AD ( K , 2) -AXX (K , J , 1 ) • AD (K , 1 ) ) • SGN 
AA (2 , 3 , 1 ) =AA (1,1,3) 

AA(I  ,2 ,2)=AXX(K,-T,3)  -ADfK,3J  -SGN -2.0 
AA(I  ,2 ,3)=-^Z-AXX(K,J,l)  -AD(K,3)  - SGN 
AA ( 1 ,2,1)= -AA (I ,2,3) 

BB ( 1 ,3,3)  = (AXX  (K , J , 2 ) • AL)  (K , 1 ) -AXX  (K , J , 1 ) • AD (K , 2)  ) • SGN , BB  ( 1 , 1 , 1 ) = -BB ( 1 , 33) 
BB(I,2,3)-  - fl- AXX ( K , J , 2) .AD(K,3) • SGN , BB(1 , 2 , 1 ) = BB( 1 , 2 , 3) 

BB ( I ,1 , 3)=-(AXX(K,J,l) • AD ( K , 2) +AXX (K, J ,2) -AD(K, 1) )SGN, 
BB(I,3,1)=-BB(1,1,3) 

BB(I  ,1  ,2)=/f.AXX(K,J,3)  • AD  ( K , 2)  • SGN , BB(  1 , 3 , 2)  = BB  ( 1 , 1 , 2) 

BBC  I ,2, 2) =0.0 


I + II 


A Subprogram 
Purpose : 


To  compute 


- (-D 


2.3 1 -J2+N 


(J1+J2-  J3)  ! ( j 2+J3-J1 ) ! (.T3+J1-J2)  ! (J3*P)  ! (J3-P) 
(J]-J2*J3+1)  ! (.1 1 +m)  ! (.11  -M)  ! (J2+N)  ! (J2-N)  ! 


}.  t (J.VJl  -N-t  ) ! (J2+N+t)  ! 

1 (J3+P-t) ! (t  +.12- J 1 -P) ! t ! (J3-J2+J1  - 1 ) ! 


• where  MI.  ”1  t < MU 

Ml.  M/O. (.(),.  Jl  + P-J2,-J2-N) 

MU  = MIN  (.13+. II  -N, J3+P.J3-J2+J1 ) 

Calling  Sequence: 

Y = C3J(J],  J2 , J3,  M,  N,  P) 


Input : 


J1 

, .12,  .13 

non-negative  integer  with  a condition 
max  (.1 1 -.12  , J2-J1)  < .13  £ J1+J2 

M. 

N,  P 

integers  with  conditions 

-J1  £ M <.  J1 
- J2  £ N £ J2 
- J3  < P £ J3 


Common  Variables: 


None . 


1 
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Special  Considerations: 

] . If  Ml)  < ML,  then  Y = 0.0 
J.  In  order  to  avoid  the  ease 

(_1)  * * o,  (for  example  (-1)  * * KJ) , 
K.)  is  replaced  by  KJ+  a large  even  integer. 
Other  Subprograms  Called: 

IK. 
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I IMN0 

A Subprogram 
Purpose : 

To  determine  a box  region  arour.d  the  mode  of  the  conditional  density 
function  I’ (.4'.  9,  ip)  = C-exp(f(<J>,  0,  iJj))  for  local  integrations. 

Calling  Sequence: 

CALL  UMN0  { P,  Q,  A,  l-MAX,  Z,  T,  W) 


Common  Variables: 

PA  1 2 

1 nput : 

PA  12 

Q 

A 


(•MAX 


Output : 

Z 

T 

W 


.7  IT 

^'lmn^’  ^2mn 2xSx5-dim  arrays,  respectively 

Mode  (<P0,  0o,  \p0)  of  the  function  0,  \p)  obtained  in 

'MAXI',  3-dim  array 

Maximum  (f(s))  on  S0(3) 

First  F.uler  angles  <j)j,  <p^>  2-dim  array 
Second  Euler  angles  0 , 0^,  2-dim  array 

Third  Euler  angles  2-dim  array 

(.Z(i),  T(i)  , W(k) ) 1 S i,  j , k S 2 represents 

coordinates  of  a corner  of  the  resulting  box  region 


Special  Consideration: 

1.  Z(1 J < A(2) , T(l)  < T(2) , W(l)  < W(2) 

2.  is  determined  by  decreasing  <(>  in  f C4> » 0O,  >P0)  until  it 
reaches  a constant  C0.  On  the  other  hand  <f>2  is  determined  by 
increasing  <f>  in  f(<f>,  0O,  \p0) . The  other  Euler  angles  0j,  82, 
4>l  , ^2  ore  obtained  similarly. 


T 


I 

I 


¥ 
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irnj  i 

A subprogram 
Purpose : 

l l 

To  assign  values  Rc(D  fZ,T,W))  or  Im(D  (Z,T,W))  to 

mn  1 mn  ^ 

£,m,n, Z,T, W 
Calling  Sequence: 

Y = DRI  (L,  M,  N,  Z,  SI),  W,  JJ) 

Common  Variables 

PA  1 2 

Input: 

PA12  constant  given  in  the  main  program 

l 

L order  of  function  Dmn(Z,T,W)  to  be  computed 

l 

M,N  indices  of  function  D^CZ.T.W)  to  be  computed 

SD  value  A1  (T) 

mnv  1 

Z,W  Kuler  angles  with  0 ± Z,W  <_  2tt 

JJ  indicator 

if  JJ  = 1,  Re(I)^n(Z,T,W))  to  be.  computed 
if  JJ=2,  Im(D^n(Z,T,W))  to  be  computed 

Special  Considerations: 

1.  SI)  must  be  precomputed  in  the  calling  program. 

This  is  taken  in  order  to  save  computing  time  in 
integration. 

Other  Subprograms  Called: 

None 


s 


A Subprogram 
Purpose : 


To  calculate  Rel)^  (.s)  and  ImD^  (s)  at  s=  (q.  ,q0  ,q_  ,q  ) and  store  the 
results  in  the  matrix  DD.  The  following  relations  are  used. 


DR I 2 - 1 


4 - n>°  - "'“111'2-0 


BeDM  ‘ 1 ‘ 


if  - ‘ Rel,1u/2  0 


1 2 2 
ReD-ir  “2  - 


1 * "Jc/4  - KoU!i^2  0 


„ nl  3 2 

ReDll  = % ‘ q3 


4 * ni0/4  * 


- ImD‘jj/2 


ReD!io“  '/2Cq1q3+q2q4) 
ReDoi  = /2(q2q4"qiq3) 


‘I  <ReD-io-ReD5iJ 


ImD_n=  _2qiq2 


■f  <««»!, o*B<!Dii> 


lmDll  ■ '"VC 


- I ml)  J j/2.0 

■1 


=-^rt,q4*q2q3) 

ImD- 10=  >/2(q2q3'qlq4) 


■'f  <I"DJr,"B!io> 


Calling  Sequence: 

CALL  DRI2 (Q,  DD) 


Common  Variables: 


None . 


Input : 


4-dimensional  array  representing  a quaternion. 
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Out  put  : 


4'  .£ 

3 '-•>  dimensional  array  for  Ref)  Is)  , lmD  (si 

mil  mn  ' 


Spot'  i ;i  1 (!»ns  idernt  ions: 


1.  The  values  ReD  , ImD  are  stored  in  the  following  way 
mn  mn  *  2  3 


ReDl)0 

ReDiu 

lm"-J0 

Renio 

R'Dm 

'■"ill 

“cUn 

2l'3''4 

q3+(|2q4)  q 

2 2 

2 - ql 

^ z 

/2(<l2q3-q,q4)  1-2(Mj+  q?)  Jl  (q„q4 -q  ^3) 

2 2 

_2qlq2  ',/2(qiq4+q2q3)  q4  ' q3 


Other  Subprograms  Called: 
"■'one. 
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EIGEN 

A Subprogram 
I'u  rpose : 

To  compute  eigenvalues  and  eigenvectors  of  a given  real  symmetric 
matrix  by  t r id i agonal i zation . 

Ca 1 1 ing  Sequence : 

CALL  1. 1 GEN  {N,  A,  E,  V) 

Common  Variables: 

None . 

I nput : 

N dimension  of  matrix  a (<.  4) 

A 2-dim.  array  containing  the  symmetric  matrix. 

Output : 

E array  containing  the  computed  eigenvalues  in  absolute 

descending  order. 

V eigenvectors  stored  in  columns. 

Special  Considerations: 

None. 

Other  Subprograms  Called: 


TRIDMX,  F.IGVAL,  EIGVEC. 


EIGEN-2 


Flow  Chart 


Return 
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1 I OVAL 

A subprogram 
I'm  pose : 

lo  compute  the  eigenvalues  of  a symmetric  tridiagonal  matrix 
using  St  urn  sequences. 

Calling  Sequence: 

CALL  1. 1 C.VAL ( l.P  , L,  A,  11,  W,  Y) 

Common  V:ir  i ah  1 cs 
None . 


I nput : 

TP  order  of  tridiagonal  matrix 

A LP-dim.  array  containing  the  diagonal  elements  of 

the  tridiagonal  matrix. 

B LP-dim.  array  containing  off-diagonal  elements  in 

R (2 ) through  B(N).  B(l)  = 0.0. 

Out  put  : 

1 LP-dim.  array  containing  the  computed  eigenvalues 

in  absolute  descending  order. 

W LP-dim.  dummy  array. 

P LP-dim.  dummy  array. 


Special  Considerations 

1.  The  diagonal  matrix  is  not  fed  into  the  subroutine  as  a 
matrix.  Instead,  the  main  diagonal  is  stored  in  the 
A array.  The  off-diagonal  elements  are  stored  in  B(2) 
through  B(N) . B(l)  must  be  equal  to  zero. 
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2,  F.1GVAL  is  designed  to  be  used  after  calling  TRIDMX.  The 
output  from  TRIDMX  is  in  the  correct  form  for  input  to 
FICVAI.. 


Other  Subprograms  ('ailed: 


None. 
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Flow  Chart 


AM  = | A ( 1)  | 

BM  0.0 

i : : 

AM- MAX (AM, | A(  I)|  ) 
BM=MAX  ( BM  , | BCD]  ) 
1 = 2,3,.  . . ,I.P 


▼ . _ 


A (1 ) =A ( I ) /BD 
B(I)  = B(n/BD 


S] 

N 
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F.  IGA' EC 

A subprogram 
Purpose : 


To  compute  the  eigenvectors  of  a real  symmetric  tridiagonal 
matrix  using  Wilkinson’s  method. 


Calling  Sequence: 


CAM.  1.1CV1C(I.P,  NM,  R,  A.  B,  E,  V,  P,  Q) 


1 nput : 


LP 

NM 


R 


A 

B 


Output : 


V 

P 

Q 


order  of  tridiagonal  matrix. 

maximum  number  of  rows  that  the  tridiagonal  matrix 
can  have  as  specified  by  the  DIMENSION  statement 
in  the  calling  program. 

NMXNM-dim.  array  containing  the  transformation  vectors 
used  to  reduce  the  symmetric  matrix  to  tridiagonal 
form. 

N-dim,  array  containing  the  diagonal  elements  of  the 
tridiagonal  matrix. 

N-dim.  array  containing  the  off-diagonal  matrix  in  B(2) 
through  B(N). 

N-dim.  array  containing  the  eigenvalues  of  the  tridiagonal 
matrix. 


NMXNM-dim.  array  containing  the  eigenvectors  stored  in 
columns  of  the  tridiagonal  matrix. 

N-dim.  dummy  array  for  temporary  storage, 

N-dim.  dummy  array  for  temporary  storage. 
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Special  Considerat ions : 

1.  EIGVEC  is  designed  to  be  called  after  TRIDMX  and  EIGVAL. 

The  transformation  vectors  needed  in  A are  computed  by 
TRIDMX.  The  tridiagonal  matrix  itself  is  not  fed  into 
the  subroutine  as  a matrix.  Rather,  the  diagonal  elements 
are  in  the  D array  and  the  off-diagonal  elements  are  in 
the  B array.  TRIDMX  will  put  those  respective  elements 

in  those  arrays. 

2.  The  accuracy  of  the  eigenvectors  is  determined  by  the 
separation  of  the  eigenvalues.  The  closer  the  eigenvalues, 
the  less  accurate  the  eigenvectors.  In  case  of  multiple 
eigenvalues,  multiple  eigenvectors  will  be  computed. 


Other  Subprograms  Called. 


None . 
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ERR 

A Subprogram 
Purpose : 

1.  To  determine  the  maximum  value  of  a function  in  the  exponent 
of  the  coiul  i t ionn 1 density  function  p(s^|m  ) and  Euler  angles 
where  the  function  is  maximum. 

2.  To  compute  the  normalizing  constant  for  pf | ) . 

3.  To  evaluate  ECq-q1)  with  respect  to  p(SjJmK)  and  to  find 

T 

the  maximum  eigenvalue  X of  E(q-q  ) and  its  eigenvector 
P so  that 

LKR1  1 - X = 1 - pTE(q-qT)p 
is  the  optimal  estimation  error. 

a /. 

4.  To  find  distances  between  s^  and  s^,  between  s^  and  s^, 
and  between  s^  and  s^  on  S0(3),  where  s^  the  optimal  es- 

a i k 

timate  and  Sj.  the  mode  of  the  p(s^|m  ). 

Calling  Sequence: 

CALL  |-:RR(1NDX,  SDD1  , SDD2  , P,  Q,  Qlll , QH2 , 1RR1 , ERH1  , ERH2 , 
ER12) 


Common  Variables: 

N,  NZ,  MT  integers  given  in  the  main  program. 


Input : 

INDX  INDX=1  If  return  without  any  computation 

INDX=2  If  the  mode  of  distribution  in  terms 
of  Euler  angles  and  quaternion  to  be  computed 
with  error  between  s.  and  f^,.  ^ 

INDX=3  If  the  maximum  eigenvalue  X of  E(q-q  ) 
and  its  eigenvector  §.  with  the  error  estimate 
between  and  in  addition  to  the  case  INDX=2. 
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spin 

3x3x17  dimensional  array,  {d^n(0)} 

Sl)l)2 

2 

5x5x17  dimensional  array  (d^f©)} 

both  are  precom]>uted  in  'INSDD', 

SINE 

53-dim.  array,  (sin(<j>)}. 

COSN 

33-dim.  array,  {cos(4>)} 
both  are  computed  in  'SINCOS'. 

P»Q 

2x5x5  dimensional  arrays  representing 
coefficients  in  y>(s,  J n>  ),  computed  in 

the  Fourier 

1 NPAQ' . 

Out  put : 


Qlll 


QII2 

ERR1 

ERH1 


F.RH2 


HR  12 


1-dim.  array,  the  unit  eigenvector  corresponding 
to  the  maximum  eigenvalue  A , s^. 

1-dim.  array,  the  mode  of  the  distribution. 

1-A,  where  A is  the  maximum  eigenvalue  of  E(q-q  ). 

distance  between  s^  = {q^ , q2>  q^,  q4)  and  = 
(Pj,  P2,  P3,  P4)  i-e.  4 

1 ■ z PiV 

i=i  1 1 

distance  between  s^  and  s^. 

A 

distance  between  s^,  s^. 


Special  Consideration 

1.  {d^n(0)}  and  (sin(4>),  cos(<j>)}  are  precomputed  to  save 
computing  time. 

2.  To  find  eigenvalues  and  eigenvectors  of  symmetric  matrix 

T 

E (q  • q 1,  first  the  matrix  is  tridiagonal ized  by  'TR1DMX', 
then  eigenvalues  and  eigenvectors  are  obtained  by  'E1GVAL', 
' EIGVEC'  in  the  subprogram  EIGEN. 


Other  Subprograms  Called 


INTGLE,  QTN,  QVQ,  EIGEN 


Row  Chart 
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ESTIM-1 


liST  IM 

A Subprogram 
Purpose : 

I . To  compute  the  mode  and  the  maximum  value  of  the  function 

k k 

f(4>,  0,  i|0  in  the  conditional  density  function  P(s  |m  ) 

2.  To  compute  the  normalizing  constant  for  P(s^|m^)by  local 
integration . 

3.  To  evaluate  the  maximum  eigenvalue  A and  its  eigenvector 

p(  = s, ) of  the  matrix  E(q-qT)  with  the  conditional  density 
i k 

function  P(S^|m  ) and  obtain  the  optimal  estimation  error 
1 - A = 1 - p1  • E ( q ■ qT ) -p 

4.  To  obtain  distances  p(sk>  sk)  , p (s^,  sk) , p (sk>  sk) , where  sk 
true  quaternions,  sk  optimal  estimates,  sk  maximum  likelihood 
estimates  by  P(skjm^). 

Calling  Sequence: 

CALL  ESTIM  (P,  Q,  QT,  QH1,  QH2,  ERR1 , ERH1 , ERH2 , ER12) 


Common  Variables: 


QS 

Input : 

QS 

sk>  real  attitude,  4-dim  array 

P,  Q 

{Plmn} ’ {P2mn}>  2x5x5-dim  arrays 

QT 

predicted  attitude  at  time  k,  4-dim  array 

Output : 

QH1 

A 

Optimal  estimate  of  sk>  sk>  4-dim  array 

QH2 

Mode  of  the  distribution,  sk,  4-dim  array 
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ERR1  Optimal  estimation  error  given  by  1-A,  where  X is  the 

maximum  eigenvalue  of  E(q-q^) 

liRlll  Pi  stance  between  s.  and  s, 

k k 

HRH2  [i  i stance  between  and  s^ 

I.R12  Distance  between  s,  and  s.  . 

k k 

Special  funs i deia t ions: 

1.  To  find  a mode  of  the  conditional  density  function,  a searching 
method  is  taken. 

2.  To  find  eigenvalues  and  eigenvectors  of  a symmetric  matrix, 
E(q-qT),  the  matrix  is  first  tridiagonal i zed  by  'TRIDMX'  and 
then  eigenvalues  by  'EIGVAL',  eigenvectors  by  'EIGVEC1  are 
obtained . 

Other  Subprograms  Called: 

MAXI,  L)MN0 , BLK0  INTGL2 , QTN,  QVQ,  EIGEN 


ESTIM-3 


art 


> 


M 
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FR-1 


I 

I 

1 

1 

I 

i 


FR 


A subprogram 
Purpose : 


To  compute  n!  tor  a given  non-negative  integer  n. 


Calling  sequence: 

Y = FR(N) . 


Input : 


N: 


integer 


Special  Consideration: 

j 1.  If  n 1 0,  N assumes  0 

! 


* 


I 

i 

t 

I 

I 


FU 

A subprogram 
Purpose : 

To  compute  the  values 


'U1 (s) 

' FU(1, 

u3(s) 

FU(2 , • , • , • ) 

-y, (s) 

FU( 3 , • , * , • ) 

y3(s) 

FU(4 ) 

u7(s) 

FU(S ) 

u2(s) 

I 

-n 

a 

as 

V — / 

1 

where  u^ (s)  (or  Uj(s))  is  given  by 

Uj (5)  = m^3a3(k)DQ0(s)  + (k)ReD^10(s) 

+ (mj 2a2 (k)  - mj1a1(k))HeD^u(s)  - *^M_. 
+ (mjja1  (k)  4 mi2a2(k))ReD1n  (s)  ] 

♦ l/2M^3a2(k)Imn^10(s)  - (nUja^k)  + m.. 
- /2M^2a3(k)ImDQ1(s)  + (m^a^k)  - m^a 


Calling  Sequence: 

Y = FU(K,  DD,  AA,  BB) 

Common  variables: 


a3(k)ReDj1(s) 

.a1(k))ImD^11(s) 

(k^ImD^fs)] 


None. 


K 


integer. 


Dl> 

AA,  BB 


values 

'01112* 


It 

u. 

"Hi 

if  K 

-1. 

FU 

It 

if  K = 

=2 

FU  = 

“Hi 

if  K 

=3, 

FU 

= 

if  K= 

=4 

FU  = 

y.2 

if  K 

=5, 

FU 

ii 

IIC 

K) 

if  K= 

:6 

’ { 

ImD1  (s) 
mn1  1 

} ’ 

computed 

in 

dimens 

i ona 

1 array. 

coefficients  of  {ReD^Cs)}  and  {ImD^s)  } , 

respectively,  computed  in  ’C0EFF1',  6x3x3 
dimensional  arrays. 


Special  Considerations: 
None. 


Other  Subprogram  Called: 
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GAUSS 

A subprogram 
Purpose : 

To  generate  a random  number  from  a given  normal  distribution 
N(m,  s)  (The  program  is  taken  from  IBM  360  scientific  subroutine 
package, p. 77) . 

Calling  Sequence: 

CALL  GAUSS (IX,  S,  AM,  V) 

Common  Variables: 

Nor.e 

Input : 

IX 

S 

AM 

Out  put : 

V 

Special  Consideration: 

1.  In  the  program,  the  mean  is  always  assumed  to  be  zero. 

Other  Subprograms  Called: 


initial  random  integer  to  create  a sequence  of 
random  numbers  from  the  uniform  distribution. 

standard  deviation. 

mean 

random  number  from  the  normal  distribution. 


RANDU 
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GAUSS4-1 


GAUSS4 

A subprogram 
I’n  rposc : 

To  j'.i'iiirat  e four  independent  random  values  from  normal  distributions. 

Cal  I in;;  Sequence  : 

CALL  GAUSS-1  (M,  1X0,  R , V) 

Common  Variables: 

None. 

Input : 

i! 

1X0 
R 

Output : 

V 

Special  Considerations: 

1.  Pour  random  integers  are  chosen  among  M integers  for  V(l), 

V (2) , V (3) , V(4) . 

Other  Subprograms  Called: 


total  number  of  times  to  use  RANDU  to  generate 
random  values  from  the  uniform  distribution. 

initial  integer  for  RANDU. 

th 

1/R(i)  representing  the  variance  of  the  i— 
normal  distribution,  1 r 5 4. 

random  values,  4-dim.  array. 


RANDU,  GAUSS. 
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GAUSS4 


Flow  Chart 
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INITl'Q-l 


INITI'Q 

A ft ubprogram 
Purpose1 : 

To  give  an  initial  density  function  for  the  conditional  density 
functions  {P  (Sj_  Jin^} } ; -e  P(s  jm°)  = Pfs  ). 

As  an  initial  density  Pfs  ),  the  following  type  of  distribution 

G 

on  S0(3)  is  chosen, 

9 

P(so)  = c-exp  (r (a + a.^  + a^D 
where  (a^,  a,,  a_,  a ^ ) is  a constant  quaternion,  represent ing  the 
mode,  and  sq  = (c^  , q2 , qv  q4). 

2 

In  the  program  fa^q^  *•  a-,q9  + + a^q^)  is  expressed  as  an 

expansion  of  (Rel)1  } { I nil)  ^ ] , and  the  coefficients  of  the  ox- 
‘ inn  mn 

pans  ion  are  stored  in  fl’0,  Q0)  . 


Calling  Sequence: 


CAM.  INITI’Q  (AK,  A,  P0,  Q0) 


Common  Variables: 


None 


Input : 


AK 

A 


parameter  representing  the  magnitude  of  mode  of 
t ho  d i st  r i hut  ion , r . 

1 dimensional  array,  mode  (a} , a,,  a„,  a^). 


Out  put : 


P0,  Q0 


2x5xS  dimensional  arrays,  coefficients  of  { RcD^  (s  ) r 
. run  o 

{ lml)^n (sq)  } , respectively. 


! 
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Special  Considernt  ions: 

1.  The  relations  between  (q^  q2 , q^,  q^)  and  { D^,nfs0^are 
given 


2 

qi 

■J  - tt>DJo-t?)"eDai 

2 

q , 

-T  - (J°Doo*  (y)Rel’ai 

<r. 

.•> 

' 4 * ‘4X0  - (i)lten!i 

9 

lU 

" J * <4L)0Jo' 

1 

V»2 

,h,  .1  /2 

-(2)imD_ii , qjq^  - 4 

<XlO 

- Xl>  i 

qiq4 

■ 1 * "<>• 

q2q3  ' ‘ 

T - 1*0? 10> 

q2q4 

= "1  (ReD-10  * Xi> 

2.  In  the  program,  the  north  pole  (0,  0,  0,  1)  is  taken  as  a mode. 

7>.  Coefficients  of  { ReD*  } and  { ImD*  } are  stored  in  P0(1,-,-) 

mn  mn 

and  Q0(1,  •,  •)  respectively.  By  the  properties  of 


(-l)n'mReD^ 

mn 

, , . n-m+1 
(-1)  ImD  , 
mn’ 


The  coefficients  are  arranged  using  the  following  signs  tables: 
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-1  1 1 

1 0 1 

1 1 1 


for  imaginary  parts 


1 


Example:  If  the  coefficient  of  ReD^j  is 


/I 


“ (a + a2a4)-r,  then 


P0(1,  2,  3)  =*y  (-aja3  + a2a4) -r 
P0(1,  4,  2)  =vy  ( a^  ▼ a2a4)-r 


this  gives  a term 


2-P0(l,  2,  3.)  ReliJj  + ip0(l,  1,  2)ReD^_1 

= 1f  ('ala3  + a2a4),r  ReD01  +,T(-aia3  + a2a4)'r  ReDll 
+ a2a4^  ’ r ReD01  (since  ReD0-l  = -ReDJi) 
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P0O.3.3) 
P0(1.1,1) 
P0d,2,3) 
P0(1 ,1,3) 

P0(l,2,2) 

P0(l,l,2) 

00(1,3,3)= 

00(1.2,3): 

00(1.1,2)= 

00(1,1.2)= 


Flow  Chart 


=(a*-a*)/2.0 

■ P0 ( 1 ,3,3) 

:(a  a4-aia3)  S2/2 .O,P0(1 ,2 , 1)= - P0 ( 1 ,2,3) 

(al'a2)/2‘O»P0(l»3,l)=P0(l,l,3) 

, 2 2 2 2. 

(a3+a4'ara2)/2-0 

(aia3+a2a4)/2/2.0,  P0(l,3,2)=  - P0 ( 1 ,1,2) 

-a3a4,  A0 ( 1 , 1 , 1 ) = -Q0 ( 1 , 3 , 3) 
-(aia4+a2a3)/2/2.0,  Q0(1,2,1)=Q0(1,2,3) 
-a  a Q0(1 ,3,1 ) =-Q0(l ,1,3) 

1 ^ t 

(a2a3-a1a4)/2/2.OfQ0(1,3,2)=Q0(l,1,2) 


3 

* 

P0(1 , I ,J)=AK*P0(1 , I ,J) 

Q0(1 , I ,J)=AK*Q0(1 , I ,J) 

3 

Return 
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INSLtI) 

A subprogram 
Purpose : 

To  generate  a table  for  {d  (0)  } , where  1 <.£.£2,  -£<m,n  <_•£ 

mn  — 

Calling  Sequence: 

CALL  INSDD  (MT,  SDD1 , SDD2)  * 


Common  Variables: 
I) 

Input : 

I) 

MT 

Output : 


SDD1 

3x3x17  dimensional 

array , 

l<4 

a> 

*-»/• 

w 

SDD2 

5x5x17  dimensional 

array , 

<4n 

(0j)}  , 

where  6^  = (tt/MT) * l 

[i-U. 

CSI 

H 

II 

....  (MT+l) 

mesh  size,  i.e.  D = tt/MT,  given  in  the  main  program, 
number  of  meshes  in[0,ir]  (in  the  program  MT=16)  . 


Special  Considerations 

1.  The  values  d1  (0.),  d2  (6:)  are  stored  in  SDDl(m+2,  n-2) 
mn  i mn  1 

SDD1 (m+2 , n+2 , I)  and  SDD2(m+3,  n+3,  I),  respectively. 


Other  Subprograms  Called: 


SD. 
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INTGLE 


A Subprogram 
Purpose : 

To  compute  the  expectation  E(q-q^)  with  respect  to  the  conditional 
dens  ity 


where 


I’(sk  |mk)  = G-cxp  (f  (sk)) 

O 

*"  I 0\. 

f(sk}  = 4=1  ^ P100D00^Sk) 


and  C is 
variable. 


* pf!>0L‘sk») 


the  normalizing  constant. 


^ £k  & 

T (P,  ReD  (s.  ) 
L „ lmn  mn  k' 

m,n=-  2, 
m<n 
or 

m=n>0 


q * (qr  q2.  q3.  q4)  quaternion 


Calling  Sequence: 

CALL  INTGLE(IND,  SDD1 , SDD2 , SINE,  COSN,  P,  Q,  FMAX,  ANG,  C,  QQ)  . 


Common  Variables: 

MT,  NZ,  H,  D 


Input : 


I 


MT 

NZ 

D 

H 


number  of  meshes  in  [0 , tt]  for  0 
number  of  meshes  in  [0,  2 it]  for  $ and  ip 
mesh  size  for  0 , i.e.  D = tt  /MT 

mesh  size  for  $ and  V » i***  H®2tt/NZ 

The  above  values  are  computed  in  the  main  program  for 
integration. 
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SUD1  , SDD2  3x3x17  and  5x5x17  dimensional  array,  representing 

{ d1  (0.)  } , {d^  (0^) }respcctively , where 

mn  l ’ ’ mn  i v ’ 

Oi  = D.(i-l),  i = l,  2 (MT+1). 

IND  integer  IND=0  if  max  (f (s^) ) (=FMAX)  on  S0(3)  to  be 

computed.  IND=1  if  a normalizing  constant  to  be  com- 
puted and  IND=2  if  E(q-q^)  to  be  computed. 

SINE,  COSN  33-dimensional  arrays,  tables  of  (sin(<fi^)},  {cos(4>j)}, 
respectively,  where  4> ^ = H-(i-l),  i = l,  2,  . ..,(NZ+1). 


ZV. 

P,  Q 2x5x5  dimensional  arrays  representing  {Pj  }, 

{ ^ 2 mn ^ respecti vely . 

Both  are  computed  in  'NPAQ' . 


Output : 

FMAX 

ANG 

C 

QQ 


MAX(f (s))  on  S0(3) 

3-dimensional  array  representing  the  mode  of  f{s)  in 
terms  of  Euler  angles. 

normalizing  constant 

T 

4x 4 dim  array,  E(q-q  ). 


Special  Considerations 

1.  In  order  to  avoid  overflow  of  computation  exp(f(<(>,  0,  I/O), 
is  replaced  by  f - fQ,  where  f = FMAX-165.0 
if  FMAX  £ 165.0  » and  fQ  = 0 if  FMAX  165.0. 


2.  Normalizing  constant  C is  computed  to  satisfy 


/ C exp(f(s)-fQ)  ds  * 1 
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.•> . 


■1 . 


The 


■'  P 


M • <1 


f at 

(k-nth 

stage  v 

kill  be  used  to  determine 

i estimate  of 

h ■ 

> 

- 

ql 

qlq2 

qlq3 

qlq4 

2 

q2ql 

q2 

°-2q3 

q2q4 

is  symmetric. 

2 

q3ql 

q3q2 

q3 

q3q4 

2 

q4ql 

q4q2 

q4q3 

q4 

only  the  upper  triangle  part  is  computed. 

I l 

S.  For  any  values  involving  ReD  , ImD  , sin  <J>,  cos  d> , tables 

mn  mn 

SDD1 , SDD2 , SINE.COSN  are  used  instead  of  calling  subprograms. 
For  example,  to  compute 

ReDmn  = COS  ^ (m'n)  ' (m<(j  + n^k^dmn^6i^ 

an  integer  MN1  is  computed,  so  that  ReD^ (<Jk  ,0.  ,ij>k)  = 

COSN(MNl) 'SDD1 (M  ♦ 2,N+2,  I).  MN1  is  determined  using  the 
following  relations, 

n NZ 

2 T 

m^+ni^k  N(J-l)  ♦ N(K-l) 

J(m-n)  - (m$.*nfk)  — (M-N)  - M(J-l)  - N(K-l) 

s MN  (mod  (NZ)) 

■ MN  if  MN  > 0 

MN+NZ  if  MN  < 0 

=»  MN1  * MN  ♦ 1 
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6. 


q-q  is  expressed  as  a 


function  matrix  of  Euler  angles 


(1  - cosO)  ( 1+cos  (<}>-i/j)/4 . 0 
(l-cos6)sin(4>-i^)/4.0  (1-cosG)  (1  -cos 

sinG(sin<Jj+s  imp)/4 . 0 s i nO  (cosiJ;-cos  4>)/4 . 0 (1  +cos0)  (1-cos  (<J>+i|>))/4 . 0 

sin0(cosi/'-cos4>J/4.0  sin0(sin4>-sini|O/4.O  (1  +cos0)sin (<J>+iJ0/4 . 0 (l+cos0)  (1+cos (<p-ip)) 

where  the  entries  of  the  upper  triangle  are  omitted. 

7.  In  the  computation  of  exp  (f-fQ),  exp  (f-fQ)  sO 
if  f-f  S -50.0  to  avoid  underflow  . 


YY 

= exp(YYl)SINE(I) 
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INTGL2 

A Subprogram 
Purpose: 

To  compute  the  normalizing  constant  C in  the  conditional  density 
function 

P(SjJmk)  = C-exp  (f(sk))  and  evaluate 
with  respect  to  P(sjJn>k). 

In  the  program,  every  integration  is  done  locally  arround  the  mode 
of  P(sjJmk) . 

Calling  Sequence: 

CALL  1NTGL2  (IND,  P,  Q,  FMAX,  A,  NZ,  NT,  NW,  H0,  C,  QQ) 

Common  Variables: 

PAI2 

Input: 

PAI2  2 it 

IND  Integer,  IND=1  if  a normalizing  constant  to  be 

computed,  IND=2  if  E(q-qT)  to  be  computed. 

0 ](  ok 

P,  Q {P,  },  {P,  },  2x5x5-dim  arrays 

lmn  lmn 

FMAX  Maximum  of  f(s)  on  S0(3) , obtained  in  'MAXI' 

A Starting  point  (Z(l),  T(l),  W(l))  for  local  inte- 

gration, 3-dim  array 

NZ,  NT,  NW  Numbers  of  mesh  in  [z(l),  Z(2)]  , [T(l),  T(2)]  , 

[W(l),  W(2)]  , respectively. 

H0  Initial  ineshsize  for  integration 

Output : 

C Normalizing  constant 

QQ  E(q*qT) , 4x4-dim  array 

"C 
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INTGL2-2 


Special  Considerations: 

1.  In  order  to  avoid  overflows  of  computation  exp  (f(<|>,  0,  i^)). 
f ( 4> , 0,  iji)  is  replaced  by  f-f0,  where  fQ  = FMAX- 165.0  if 
FMAX  > 165.0  and  fQ=0,  if  FMAX  f 165.0.  Also  to  avoid  under- 
flows, exp(f-fQ)=0. 0,  if  f-fo5-50.0. 

2.  Normalizing  constant  C is  computed  to  satisfy 

/ C • exp(f(s)-f0)  ds  = 1. 

3.  Local  integrations  are  done  in  four  steps.  At  each  step  except 
the  last  one,  the  Rieinann  integration  is  done  numerically  skip- 
ping the  1/3  x 1/3  x 1/3  of  the  original  box  region  around  the 
mode  (center)  and  the  skipped  box  region  will  become  a new  box 
region  for  the  following  step.  The  stepsize  will  be  one  half  of 
the  previous  one.  At  the  4th  step,  the  numerical  integration  is 
done  on  the  whole  box  region  without  skipping  the  middle  region. 

Other  Subprograms  Called: 

FP 


* 
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Flow  Chart 


INTGL2-3 


MiG  IN 


PAI  = PAI2/2 
C0  = -50.0 
H = H0 


Location  of  mode 

L0  = (NX/2)  + 1 
M0  = (NT/2)  + 1 
N0  = (NW/2)  + 1 


Setting  a parameter 
for  scaling  down 

f (<f>,  e,  M 

F0  = FMAX-165.0 


F0  < 0.0 


Initialization  of 
S,  QQ,  LL 

S = 0.0,  QQ ( I , J ) = 0.0 
LL  = 1 


F0  = 0. 0 


Volume  Element 
HD  = H-H-H 
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INTGL2-4 


W(l,  1)  = (l-cos(Tl)) • ( l+cos(XJ))/4.0 
W(2,  2)  = (l-cos(Tl)) • (l-cos(Xl))/4. 0 
W(3,  3)  = (l+cos(Tl)) • (l-cos(X2))/4.0 
W(4,  4)  = (l+cos(Tl)) • (l+cos(X2))/4.0 
W(l,  2)  = (1-cos (Tl)) • sin(Xl)/4 . 0 
W(l,  3)  = sin(Tl) (sin(Xl)+sin(Wl))/4.0 
W(l,  4)  = sin(Tl) ■ (cos (Wl)+cos(Xl))/4. 0 
W(2,  3)  = sin(Tl) • (cos (Wl) -cos(Xl) )/4 . 0 
W(2,  4)  = sin(Tl) • (sin(Zl) -sin(Wl) )/4 . 0 
W(3,  4)  = (1.0+cos(Tl) ) sin(X2)/4.0 
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MAXI-1 


MAXI 

A Subprogram 
Purpose : 

To  locate  the  mode  of  a function  f(<J>,  0,  i(>)  in  the  conditional 
density  function. 

P($,  0>  4<)  = C exp  (f(<j>,  6,  '{'))>  where  C is  a 
normalizing  constant. 

MAXI  (P,  Q,  QT,  B,  FMAX) 


2ir 

<0-  {p2»„k  2x5xs-di*  *rr*>’s 

Initial  quaternion  to  start  for  searching, 

4-dim  array 

Mode  in  terms  of  Euler  angles,  3-dim  array 
Value  of  the  function  f (4> , 6,  ^i)  at  the  mode. 

Special  Considerations: 

1.  Two  starting  points  are  used  to  locate  the  mode.  The  first 
point  (A(l),  A(2),  A(3))  is  given  by  the  calling  program  and 
the  second  point  may  be  given  by  (A(l)+n,  A(2)+ir/2,  A(3)+ir). 
This  is  to  avoid  locating  a local  maximum  instead  of  the  global 


Calling  Sequence: 

CALL 

Common  Variables: 

PAI2 


Input: 


PAI2 
P.  Q 
QT 


Output : 


B 

FMAX 


maximum. 


127 


MAXI -2 


Other  Subprograms  Called: 

QTA,  Q IN , FP,  SI-ARCH 


^ Blit  1 1 N 

T 

U’SI  0. 00001 
NX  ^ 2 

MX  = 300 


Initial  quaternion 
into 

Euler  angles 
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Flow  Chart 


MAXI -3 


Initial  mesh  si„e 
for  searching  maxi  muni 

DMESH  = PAI2/8 


A(i)  = C(k,i) 
i = 1,  2,  3 


C(l,i)  * A(i) 
i = 1,  2,  3 


Setting  another  starting 
point 

A(  1)  = A(  1)  + it 
A(2)  = A(2)  * */2 
A(3)  = A (3)  + 7T 


JJ  = 1 


Initial  value 
at  1st  point 


= JJ+1 


Normalization  of 
2nd  point 
CQTN , QTA) 


Mode  by 
1st  Point 
(2nd) 
(SEARCH) 


C (2 , i)  = A(i) 
1 5 i 5 3 


DMESH  < 


JJ  > MX 


-Vnf  5 nx\JL 


w \ 1 

DMESH  = DMESH/ 2 



3 
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M06PLT- 


M0GPI.T 
M in  |iii> ram 


To  read  pmi  : Is^}  , { } , 1 } and  sequences  of  error 

estimates  from  the  disk  and  draw  yraph  . 


Common  Variables: 


None . 


I nput : 

A 1 7 l d-d  i in . array  containing  all  the  values. 

Special  (lonsiderat  ions: 

1.  "roec  :.es  , s s^  arc  called  QSN,  QIU  , QH2  in  MflS.SO.S 
and  stored  in  the  location. 


A (568)  - > QSN (1,1) 
A(768)  , > QH1 (1,1) 
A(968)  QH2  (1,1) 


7 


Optimal  estimate  errors,  p (s^, 
p(s^,  Sj, ) are  called  TURN,  F.RH1 
and  stored  in  the  location 


) > ond  ». •(  ) , 

, HR1I2,  KR12  in  M05SO3 


A (268)  - - I.RRN  ( 1 ) 

A(3] 8)  liRJIl  ( 1 ) 

A (368)  « ► 111112(1) 

A (4 1 8)  111112(1) 

3.  Quaternion  representation  QSN,  QH1 , QH2  of  s^, 

are  modified  so  that  4th  component  of  each  quartern  ion 
is  posit ive. 


I 

! 


131 


M06P1.T-2 

•>t  hfr  •■.tih|ir<.->r;„!,;. 

I’i.oi  i . 


1 

« 

t 

1 
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NEW0- 


NEWrf 

A subprogram 
Purpose : 

To  generate  C in  the  process. 
k+1  k k 

using  a pseudorandom  number  generator. 

Calling  Sequence: 

CALL  NEW  <J>  (1X4),  PAI2  , W4>) 


Common  Variables: 

None 


Input : 

IXO 

PAI2 


initial  random  integer 
2tr 


Output 

W0 


3 dim.  array  representing  Euler's  angles 

4>,e,^. 


Special  Considerations 

1.  Only  three  numbers  are  chosen  from  the  start  by  RANDU, 
Other  Subprogram  Called: 


RANDU. 


- 135  - 


NPAQ- 1 


NI'AQ 

a • ubjiro;  i ni  i 
Purpose  . 


To  update  (P^1)  and  {P^1}  of  Pf*^  |mk-1) 

to  {pfk  } and  {P^k  } of  P(s.  |mk) , where 
Imn  2mn  k 

9 

nsk|",ki  - ( ,xp  l jff50"„o(sk)  * )■' yiLM'Llh> 

K.~  1 in.T)--c 

iflMl 

O ** 

m=n>0 


£k  l 

* P.,  ImP  (s.)) 
2mn  mn  k 


Call  ini;  Sequence: 

CAM,  NPAQ(PR) 

Common  Variables 

AA,  BB,  W0,  OBN,  P0,  Q0,  PI  Q1 


Input 


RU 

AA,  BB 
OBN 


constants  given  in  the  main  program. 

coeffic  ients  of  the  functions  (-tij,  u^,  -Uj , u^,  u^,  u^) 
observed  value  computed  in  the  main  program 
6-dim.  array,  {m£} 


P0 

, £k-l 
1 lmn 

Q0 

/p^k-1 

1 2mn 

Output : 


PI 

{P*k 

lmn 

Q1 

{p«. 

' 2mn 
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NPAQ- 4 
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PLOTl-l 


I 
I 

| PLOT1 

A subprogram 

i 

Purpose: 

To  draw  one,  two  or  three  graphs  with  different  type  of  lines. 
Calling  Sequence: 

CALL  PL0T1  (M,  N,  X,  Y,  Z,  W,  IJ) 

Common  Variables: 

, None. 

Input : 

i 

j 

M number  of  graphs  to  be  drawn 

j N number  of  points  to  be  drawn 

Y,Z,W  values  for  graphs 
U a dummy  array 

i Special  Consideration 

1.  X-coordinates  are  determined  by 

( 

X(I)  = I 

. 2.  More  than  one  graph  in  one  picture  are  drawn  completely 

checking  the  size  of  y-values  using  UPDWN. 

Other  Subprograms  Called: 

i 
1 

t 

I 


UPDWN,  IN1TT,  BINITT,  NPTS,  CHECK,  DSPLAY,  LINE,  CPLOT,  TINPUT. 
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PL0T1-1 


Flow  Chart 
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or  a - 1 


qta 

A Subprogram 
I'u niw.c- : 


To  convert  a qua t i-rn ion  to  Euler  angles. 


Calling  Sequence: 

CALL  QTA  (Q,  A) 

Common  Variables: 

None 


Input : 


Q 


Output : 

A 


Quaternion,  4-dim  array 


lai  1 er  angles,  3-dim  array 


Special  Considerations: 

1.  The  following  relations  between  a quaternion  and  Euler  angles 
are  used. 

qj  = sinf  cos  ((♦-i|»)/2) 
q2  = sinf  sin  ((♦-*)/2) 
q3  = cosf  sin  ((♦♦*)/2) 
q4  = cosf  cos  ((♦♦♦)/2) 

2 2 

2.  If  Arcos(  2(q^  + q^)  - 1)  < EPS!,  then  0 is  assumed  to  be  zero. 
Other  Subprograms  Called: 


None. 


XI  = Q(2)/DS 
X2  - Q(3)/DC 
Y1  = Q(1)/DS 
Y2  - Ql4)/l)C 

d> 


QTA-2 

la  rt 


>307 1 79SH(<[l(i 
J. 0001 





RETURN 
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QTN-1 


QiN 

A Sulij  lograri 

t I I f C ’ ' 

In  transform  liuli  '•  angles  to  a < . i : : 1 1 » i i . > 1 1 

< ■ I I i nj;  Scqm  iia' 

( Al  l.  QI\|  \.  Q) 

('f.nmon  Variables 
None . 


I nput 


A 


Out  put 


Q 


l-.uler  angles  (4>,6,t)»  3 dim.  array. 


Quaternion  (q^  , q7,  q^,  q^),  1-dim.  array. 


Special  Considerations 

1.  The  formula  for  the  transformat  ion  i •.  given  by 


0 <p-ip 

= sin  j cos  -~ 


sin 


0 . <p-ip 

2 51n  2 


0 <J>+il) 

cos  - sin  2~ 


9 


0 
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QVQ- 


•\  subprogram 
Purpose : 

To  compute  the  distance  between  two  quat en i tuts . 

< a I 1 i iii'.  Sequence  : 

CM. I.  ( )\l»  ( I . i,i,  I’i 

Common  Variables: 

None 


I nput : 

P;  Q quaternions , 4-dim.  arrays. 

Output : 

1*  distance  between  P and  Q. 

Special  Considernt ion: 

1.  Distance  between  two  quntenions  pand  q is  given  by 

4 

‘1  = 1 - l Pi  * qA  . where  p - (Pj  , P2,  Pj,  P4) 

q = (qj . q2.  qv  q4) 

Otltei  Subprograms  Called: 


None. 
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QXQ-l 


QXQ 

A subprogram 
Purpose : 

To  compute  the  product  of  two  quaternions 

(r]>  r2  * 1 3 > r4 ) _ » *'2  ’ ^ 3 » ^ 4 ) ■ j > H ->  > q - , c|  j ) , 

where 

rl  = Piq4  + P2q3  - P3q2  + P4ql 


r2  = Pl^S  + P2q4  + P3ql  + P4q2 


r3  = Piq2  - P2qi  ♦ P3q4  ♦ P4q3 


r4  =-Plql  - P2q2  ' P3q3  + P4q4 


Calling  Sequence: 

CALL  QXQ(P,  Q,  R) 

Common  Variables: 

None. 


Input 


P 

Q 


Output : 


quarternion  (Pj , P2,  P3,  P ),  4 -dim.  array, 

quarternion  (qj , q2>  q3>  q4 ) , 4-dim.  array, 

quarternion  (Tj , r2,  r3,  r ) , 4-dim.  array. 


Special  considerations 

1.  Considering  computer  round-off  error,  product  quarternior 


R is 


re -norm'll  ized. 
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RAN DU-1 


RANDU 

A subprogram 
Purpose 

To  compute  a pseudorandom  integer  and  value  or  uniform  dis- 
tribution. 

The  program  uses  the  properties  of  FORTRAN  TV  and  its  admissible 
31 

maximum  integer  2'  -1  (=2147483647)  and  overflows,  (the  program 
is  taken  from  IBM  360  scientific  subrout  ine,p. 77) . 


6a  1 1 i ng  Sequence : 

CALL  RANDU (IX,  1Y,  YFL) 

Common  Variables: 

None. 


Input : 


IX 


integer 


Output : 


IY 

YFL 


pseudorandom  integer 


,31 


pseudorandom  value,  YFL  = IY/(2"  -1) 


Special  Considerations: 

1.  To  generate  a sequence  of  pseudorandom  numbers  RANDU  can  be 
used  repeatedly  by  setting  IX  = 1Y. 
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‘-INCUS  - 1 


SI NCOS 

A subprogram 
I'm  pose 

lt>  construct  immc-rical  tables  of  ■ inv,  rose  Cor  o • \ ■■  ’/i 

Ca  llinr  soipience : 

CALL  S I NCOS ( N , SINK,  COSN) 


Common  Variables: 

None 


Input : 

N 


Number  of  points  in  [0,  2ir]  to  be  taken 


Out  put : 

SINK  values  {sin  x.  } , N-dim.  array 

COSN  values  {cos  x. } , N-dim.  array 

Special  Considerations: 

1.  Tables  are  used  to  reduce  computing  time. 

2.  In  order  to  avoid  future  underflows, 

if  | SINE  (I  ) j ([  COS  NCI)  | ) < 10'11,  then 
SINE ( I)  = 0 (COS  N(I)=0) 

Other  Subprograms  Called: 


None. 
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SD 

A bprogrnm 
Purpose : 

To  3ssign  values  dl  OJ  to  each  £,m,n  and  0 ■ T y.  i. 

" mn 

Calling  Sequence: 

Y = SD(I.,M,N,T) 

Input : 

j order  of  Fourier  coefficients  to  be  computed 

indices  of  Fourier  coefficients  with  the  range 
-I.  < M, N <_  L 

T angle  0 < T < n 

Special  Considerations: 

1.  L,M,N  must  satisfy  the  relations 
0 <.  L <_  LMAX  and  -L  < M,N  <_  L 
Other  Subprograms  Called: 


t-R 
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SEARCH- 1 


' V xliljilUi  I'.llll 


find  t lie  mode  ol  the  eoiidition.il  density  function  will 


1 1 1 • t i a 1 starting  point  and  mesh  size. 


Ca  1 1 i ng  seiju  . i*  e : 


Common  Variables: 


nput  : 


SLARCII  (DMESH , N!  , I',  Q,  A,  B,  SI) 


DM1:  SH 


Slepsizc  to  search  the  maximum  value  of  the  density 
function. 


Output : 


Maximum  searching  times  in  one  direction  at  the 
previous  stage. 

{P?^  },  ),  2x5x5-dim  arrays 

lmn  2mn 

Starting  point  (Euler  angles),  3-dim  array 
Value  of  f (4>,  0,  iji)  at  A 


Maximum  point  (mode)  of  the  function  f ( 4> , n,  iji)  with 
respect  to  the  given  mesh 

Maximum  value  of  f (<(> , 0,  i(i) 

Maximum  searching  times  at  this  stage,  which  will 
be  used  in  calling  program  ’MAXI1  to  determine  the 
new  mesh  size 


Special  Considerations: 


1.  Searching  the  mode  continues  as  long  as  the  previous  function 


value  is  less  than  the  new  value. 
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TRANS- 1 


A subprogram 
Purpose : 


1 . To  store  processes 

{s.}  is.}  (s . } 1 <.  i <_  (k-1),  where  { s . } 

i i l l 

arc  attitudes,  {s.}  attitude  estimates,  {s.}  maximum 
l i 

likelihood  estimates,  and 

F.RRN(i)  = H(p(s-  , s.)),  r-RHl  (i)  = p(s.,  s.)  , 

ER1I2  (i)  = p(si,  s.)  HR  1 2 ( i ) = p(s.,  s.) . 

2.  To  save  the  values  of  the  parameters  for  the  next  step  k. 

3.  To  read  all  the  results  from  disk  and  printout  if  requested. 


Calling  Sequence: 

CALL  TRANS  (ID,  1X0,  IY0,  KK,  MM) 


Common  Variables: 

AXX , AD,  AA,  BB,  W 6; 

OBN,  P0,  Q0,  AK,  DIR,  QW,  QSN,  QH1 , QI12 , LRRN , 
F.R1U,  F.R1I2,  ER12 . , SINE,  COSN 


Input : 


ID  indicator 

the  values  are  to  be  stored  in  the  disk  if  1D=1. 

the  values  are  to  be  read  from  *the  disk  if 

ID  = 2 or  3.  If  ID=3,  the  results  are  printed. 

AXX,  AD,  AA,  BB,  W0,  OBN,  P0,  Q0,  AK,  DIR,  QW,  QSN,  Qlll  , Q1I2 , ERRN , 
ERH1 , ERH2,  ER12,  SINE,  COSN,  given  in  the  main  program. 

lXfi  integer,  the  last  integer  used  to  create  a random 

integer  by  RANDU. 

IY0  integer,  the  last  integer  used  to  create  another 

random  integer  by  RANDU. 


- 159  - 


TRANS *2 


KK  time  k at  which  all  the  result  should  be  updated. 

MM  integer,  (MM-1)  values  are  stored  for  each  QSN,  Qlll , 

QH2,  ERRN,  F.RH1 , ERH2 , F.R12  for  the  output  purpose. 


Output : 


AXX , AD,  AA,  BB,  W0,  P0,  Q0,  AK,  DIR,  QW,  QSN,  QH1 , QH2 , ERRN, 
ERH1 , ERH2 , ER12,  SINE,  COSN,  1X0,  IY0,  KK,  MM. 


Special  Considerations 


1.  For  the  use  of  disk,  variables  A,  B are  used  to  store  results 


with 

A fl 742) , B(871 , 2),  EQUIVALENCE(A(1)  , B(1 , 1) ) . 


All  the 

values  are 

stored 

in  A in 

the  following 

order 

A ( I ) : 

KK 

A (168) 

Pj9(l,l,l) 

(50) 

A ( 2 ) : 

MM 

A(218) 

Q1  (1,1,1) 

(50) 

A(3) : 

1X0 

A (268) 

ERRN(l) 

(50) 

A(4): 

IY0 

A(318) 

ERH1 (1) 

(50) 

ACS) : 

AK 

A(368) 

ERH2 (1) 

(50) 

A ( 1 1 ) : 

AXX91 ,1,1) 

(18)* 

A(418) 

ER12  (1) 

(50) 

A(29): 

AD ( 1 , 1 ) 

( 6) 

A(468) 

Blank 

(100) 

A (35)  : 

AA(1 , 1 , 1 ) 

(54) 

A(568) 

QSN (1,1) 

(200) 

A (89)  : 

BB(1,1,1) 

(54) 

A(768) 

QHl(l,l) 

(200) 

A(143) : 

RR(1) 

( 6) 

A(968) 

QH2(1,1) 

(200) 

A(149) 

DIR(l) 

( 4) 

A (1 168) 

Blank 

(400) 

A ( 1 53)  : 

W0(1) 

( 3) 

A(1568) 

: SINE(l) 

(33) 

A(156)  : 

QW(l) 

( 4) 

A ( 1 601) 

: COSN ( 1 ) 

(33) 

A(160) : 

QS(l) 

( 4) 

A( 164)  : 

QH0 ( 1 ) 

( 4) 

* 18  = 2x3x3  representing  the 
array  of  AXX  and  A ( 1 1 ) 
corresponds  to  AX  (1,1,1) 
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TRIDMX-1 


TRJDMX 

A.  subprogram 
Purpose : 

To  transform  a real  symmetric  matrix  to  tridiagonal  form  using 
Householder's  method. 

Calling  sequence: 

CALL  TRIDMX(N,  NM,  A,  D,  B) 

Common  Variables: 

None , 


Input : 

N number  of  rows  and  columns  in  matrix  A 

Also  N is  the  number  of  elements  in  D 
and  B 

NM  Maximum  number  of  rows  A can  have  as  specified 

by  the  DIMENSION  statement  in  the  calling  pro- 
gram. 

A NXN-dimensional  array  containing  the  symmetric 

matrix . 

Output : 

D array  containing  the  diagonal  elements  of  the 

tridiagonal  matrix. 

B array  containing  the  off-diagonal  elements  of 

the  tridiagonal  elements  of  the  tridiagonal 
matrix  in  locations  B(2)  through  B(N).  B(1)=0.0. 

Special  Considerations: 

1.  The  lower  triangular  half  of  A is  changed  by  TRIDMX. 

2.  TRIDMX  is  designed  to  be  used  with  EICVAL  and  EIGVEC. 


Other  Subprograms  Called. 
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IJPDWN-  i 


UPDWN 

A subprogram : 
Purpose : 


To  obtain  maximum  or  minimum  of  two  values. 


Calling  Sequence: 

UPDWN (ID , X, Y) 


Common  Variables: 

None . 

Input : 

I[)  integer,  if,  1D=1,  minimum,  if,  1D=2, 

of  two  values  X and  Y to  be  computed. 

X,  Y two  values 


Special  Consideration: 
None . 


Other  Subprograms  Called: 


maximum 


None . 


ir  cl  mi  i 
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simulation  result:;  indicate  that  the  scheme  is  effective  and  robust.  It 
is  believed  that  this  approach  is  applicable  to  many  other  attitude  measuring] 
units.  As  no  linearization  and  approximation  are  necessary  in  the  approach, 
it  is  ideal  for  systems  involving  high  levels  of  randomness. 


When  a system  involves  little  randomness  and  linearization  is  not  expected 
to  incur  much  error,  the  approach  can  provide  a benchmark  against  which  sucb 
suboptimal  estimators  as  the  extended  Kalman  filter  and  the  least-squares 
estimator  can  be  compared.  In  this  spirit,  simulated  data  for  HEAO-A  were 
processed  to  compare  the  optimal  scheme  ar.d  the  extended  Kalman  filter.  The 
results  are  presented. 
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